<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>traj_gen: src/PolyTrajGen.cpp Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { init_search(); });
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">traj_gen
   &#160;<span id="projectnumber">2.0</span>
   </div>
   <div id="projectbrief">trajectory generation module</div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="pages.html"><span>Related&#160;Pages</span></a></li>
      <li><a href="namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
  <div id="navrow2" class="tabs2">
    <ul class="tablist">
      <li><a href="files.html"><span>File&#160;List</span></a></li>
      <li><a href="globals.html"><span>File&#160;Members</span></a></li>
    </ul>
  </div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="dir_68267d1309a1af8e8297ef4c3efbcdba.html">src</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="headertitle">
<div class="title">PolyTrajGen.cpp</div>  </div>
</div><!--header-->
<div class="contents">
<a href="_poly_traj_gen_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="_poly_traj_gen_8h.html">traj_gen/PolyTrajGen.h</a>&quot;</span></div><div class="line"><a name="l00002"></a><span class="lineno">    2</span>&#160;</div><div class="line"><a name="l00003"></a><span class="lineno">    3</span>&#160;<span class="comment">// constructor </span></div><div class="line"><a name="l00004"></a><span class="lineno"><a class="line" href="class_path_planner.html#a376f30d795cfe0a40f8923f49336f7da">    4</a></span>&#160;<a class="code" href="class_path_planner.html#a376f30d795cfe0a40f8923f49336f7da">PathPlanner::PathPlanner</a>():is_path_computed(false){};</div><div class="line"><a name="l00005"></a><span class="lineno">    5</span>&#160;</div><div class="line"><a name="l00006"></a><span class="lineno">    6</span>&#160;</div><div class="line"><a name="l00007"></a><span class="lineno">    7</span>&#160;<span class="comment">// R world to b (Rwb)</span></div><div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;Affine3f PathPlanner::get_affine_corridor(Point p1,Point p2){</div><div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160;</div><div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;    Affine3f Twb;</div><div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;</div><div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;    <span class="comment">// 1. rotation matrix </span></div><div class="line"><a name="l00013"></a><span class="lineno">   13</span>&#160;</div><div class="line"><a name="l00014"></a><span class="lineno">   14</span>&#160;        <span class="comment">// e1 : x-axis of body axis attached to the centroid of 3D rectangle </span></div><div class="line"><a name="l00015"></a><span class="lineno">   15</span>&#160;        Vector3f e1(p2.x - p1.x, p2.y - p1.y , p2.z - p1.z );     </div><div class="line"><a name="l00016"></a><span class="lineno">   16</span>&#160;        <span class="keywordtype">float</span> box_l = e1.norm();</div><div class="line"><a name="l00017"></a><span class="lineno">   17</span>&#160;        e1.normalize();</div><div class="line"><a name="l00018"></a><span class="lineno">   18</span>&#160;        </div><div class="line"><a name="l00019"></a><span class="lineno">   19</span>&#160;        <span class="comment">// e2 : y-axis</span></div><div class="line"><a name="l00020"></a><span class="lineno">   20</span>&#160;        Vector3f e2;</div><div class="line"><a name="l00021"></a><span class="lineno">   21</span>&#160;        e2(2) = 0; e2(1) = 1; <span class="comment">// for simplicity </span></div><div class="line"><a name="l00022"></a><span class="lineno">   22</span>&#160;        e2(0) = - e1(1)/e1(0) * e2(1);</div><div class="line"><a name="l00023"></a><span class="lineno">   23</span>&#160;        e2.normalize();</div><div class="line"><a name="l00024"></a><span class="lineno">   24</span>&#160;                <span class="comment">// e3 : z-axis </span></div><div class="line"><a name="l00025"></a><span class="lineno">   25</span>&#160;        Vector3f e3 = e1.cross(e2);</div><div class="line"><a name="l00026"></a><span class="lineno">   26</span>&#160;        </div><div class="line"><a name="l00027"></a><span class="lineno">   27</span>&#160;        Matrix3f Rwb; </div><div class="line"><a name="l00028"></a><span class="lineno">   28</span>&#160;        Rwb.block(0,0,3,1) = e1; Rwb.block(0,1,3,1) = e2; Rwb.block(0,2,3,1) = e3;</div><div class="line"><a name="l00029"></a><span class="lineno">   29</span>&#160;        Vector3f pnt1(p1.x,p1.y,p1.z),pnt2(p2.x,p2.y,p2.z);  </div><div class="line"><a name="l00030"></a><span class="lineno">   30</span>&#160;</div><div class="line"><a name="l00031"></a><span class="lineno">   31</span>&#160;        Affine3f rot(Rwb); </div><div class="line"><a name="l00032"></a><span class="lineno">   32</span>&#160;        Affine3f trans(Translation3f((pnt1 + pnt2)/2));</div><div class="line"><a name="l00033"></a><span class="lineno">   33</span>&#160;</div><div class="line"><a name="l00034"></a><span class="lineno">   34</span>&#160;        Twb = trans * rot;</div><div class="line"><a name="l00035"></a><span class="lineno">   35</span>&#160;</div><div class="line"><a name="l00049"></a><span class="lineno">   49</span>&#160;    <span class="keywordflow">return</span> Twb; </div><div class="line"><a name="l00050"></a><span class="lineno">   50</span>&#160;}</div><div class="line"><a name="l00051"></a><span class="lineno">   51</span>&#160;</div><div class="line"><a name="l00052"></a><span class="lineno">   52</span>&#160;</div><div class="line"><a name="l00053"></a><span class="lineno">   53</span>&#160;<span class="comment">// path generation core rountine </span></div><div class="line"><a name="l00054"></a><span class="lineno"><a class="line" href="class_path_planner.html#a197058bceddd7a22cd23c74239562754">   54</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="class_path_planner.html#a197058bceddd7a22cd23c74239562754">PathPlanner::path_gen</a>(<span class="keyword">const</span> <a class="code" href="_poly_traj_gen_8h.html#a37d0493e73d8ef72a2405596ad6cee0c">TimeSeries</a>&amp; knots ,<span class="keyword">const</span> nav_msgs::Path&amp; waypoints,<span class="keyword">const</span> geometry_msgs::Twist&amp; v0,<span class="keyword">const</span> geometry_msgs::Twist&amp; a0,<a class="code" href="struct_traj_gen_opts.html">TrajGenOpts</a> opt ){</div><div class="line"><a name="l00055"></a><span class="lineno">   55</span>&#160;    </div><div class="line"><a name="l00056"></a><span class="lineno">   56</span>&#160;    is_this_verbose = opt.<a class="code" href="struct_traj_gen_opts.html#ab4888a8e142d514867bb3dfacb84e3d2">verbose</a>;</div><div class="line"><a name="l00057"></a><span class="lineno">   57</span>&#160;    <span class="keywordtype">int</span> n_seg = waypoints.poses.size() - 1;</div><div class="line"><a name="l00058"></a><span class="lineno">   58</span>&#160;    <span class="keywordtype">int</span> poly_order = opt.<a class="code" href="struct_traj_gen_opts.html#a538985230ef4a8cd7763c91d11bebbaf">poly_order</a>;</div><div class="line"><a name="l00059"></a><span class="lineno">   59</span>&#160;    <a class="code" href="struct_q_p__form__xyz.html">QP_form_xyz</a> qp_xyz = <a class="code" href="class_path_planner.html#a4b5f5beea2c380ae4340f65dd1d5bd10">qp_gen</a>(knots,waypoints,v0,a0,opt);</div><div class="line"><a name="l00060"></a><span class="lineno">   60</span>&#160;    <span class="keywordtype">bool</span> is_ok_x,is_ok_y,is_ok_z;</div><div class="line"><a name="l00061"></a><span class="lineno">   61</span>&#160;</div><div class="line"><a name="l00062"></a><span class="lineno">   62</span>&#160;    <a class="code" href="class_poly_spline.html">PolySpline</a> spline_x = <a class="code" href="class_path_planner.html#a35e8872f5ffc5fc3227dc45f9f969a85">get_solution</a> (<a class="code" href="class_path_planner.html#a6fd581289799d1daac012050fb712a0c">solveqp</a>(qp_xyz.<a class="code" href="struct_q_p__form__xyz.html#a3a6e7a104ef31fda66007013ba3741f8">x</a>,is_ok_x),poly_order,n_seg);</div><div class="line"><a name="l00063"></a><span class="lineno">   63</span>&#160;    <a class="code" href="class_poly_spline.html">PolySpline</a> spline_y = <a class="code" href="class_path_planner.html#a35e8872f5ffc5fc3227dc45f9f969a85">get_solution</a> (<a class="code" href="class_path_planner.html#a6fd581289799d1daac012050fb712a0c">solveqp</a>(qp_xyz.<a class="code" href="struct_q_p__form__xyz.html#a3e1c8fbfd5a4650fb45fc0ca14c2bcb6">y</a>,is_ok_y),poly_order,n_seg);</div><div class="line"><a name="l00064"></a><span class="lineno">   64</span>&#160;    <a class="code" href="class_poly_spline.html">PolySpline</a> spline_z = <a class="code" href="class_path_planner.html#a35e8872f5ffc5fc3227dc45f9f969a85">get_solution</a> (<a class="code" href="class_path_planner.html#a6fd581289799d1daac012050fb712a0c">solveqp</a>(qp_xyz.<a class="code" href="struct_q_p__form__xyz.html#a243478dd3ac49ecf0d6fceffbcdf408b">z</a>,is_ok_z),poly_order,n_seg);</div><div class="line"><a name="l00065"></a><span class="lineno">   65</span>&#160;</div><div class="line"><a name="l00066"></a><span class="lineno">   66</span>&#160;        <span class="comment">// interpret the solution and rescaling</span></div><div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;        cout&lt;&lt;<span class="stringliteral">&quot;------------solution in increasing order---------------&quot;</span>&lt;&lt;endl;</div><div class="line"><a name="l00068"></a><span class="lineno">   68</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> k=0;k&lt;n_seg;k++){</div><div class="line"><a name="l00069"></a><span class="lineno">   69</span>&#160;            cout&lt;&lt;<span class="stringliteral">&quot;segment &quot;</span>&lt;&lt;k+1&lt;&lt;endl;</div><div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;            cout&lt;&lt;<span class="stringliteral">&quot;x: &quot;</span>;</div><div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> n = 0; n&lt;=poly_order ; n++){</div><div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;                spline_x.poly_coeff[k].coeff[n] /= pow(knots[k+1]-knots[k],n);</div><div class="line"><a name="l00073"></a><span class="lineno">   73</span>&#160;                printf(<span class="stringliteral">&quot;%.4f , &quot;</span>,spline_x.poly_coeff[k].coeff[n]);</div><div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;            }</div><div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;            cout&lt;&lt;endl;</div><div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;            cout&lt;&lt;<span class="stringliteral">&quot;y: &quot;</span>;</div><div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160;            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> n = 0; n&lt;=poly_order ; n++){</div><div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;                spline_y.poly_coeff[k].coeff[n] /= pow(knots[k+1]-knots[k],n);</div><div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;                printf(<span class="stringliteral">&quot;%.4f , &quot;</span>,spline_y.poly_coeff[k].coeff[n]);</div><div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;            }</div><div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;            cout&lt;&lt;endl;</div><div class="line"><a name="l00082"></a><span class="lineno">   82</span>&#160;            cout&lt;&lt;<span class="stringliteral">&quot;z: &quot;</span>;</div><div class="line"><a name="l00083"></a><span class="lineno">   83</span>&#160;            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> n = 0; n&lt;=poly_order ; n++){</div><div class="line"><a name="l00084"></a><span class="lineno">   84</span>&#160;                spline_z.poly_coeff[k].coeff[n] /= pow(knots[k+1]-knots[k],n);</div><div class="line"><a name="l00085"></a><span class="lineno">   85</span>&#160;                printf(<span class="stringliteral">&quot;%.4f , &quot;</span>,spline_z.poly_coeff[k].coeff[n]);</div><div class="line"><a name="l00086"></a><span class="lineno">   86</span>&#160;            }</div><div class="line"><a name="l00087"></a><span class="lineno">   87</span>&#160;            cout&lt;&lt;endl;</div><div class="line"><a name="l00088"></a><span class="lineno">   88</span>&#160;        }</div><div class="line"><a name="l00089"></a><span class="lineno">   89</span>&#160;    </div><div class="line"><a name="l00090"></a><span class="lineno">   90</span>&#160;    is_path_computed = <span class="keyword">true</span>;</div><div class="line"><a name="l00091"></a><span class="lineno">   91</span>&#160;</div><div class="line"><a name="l00092"></a><span class="lineno">   92</span>&#160;    <span class="comment">// finalizing the path </span></div><div class="line"><a name="l00093"></a><span class="lineno">   93</span>&#160;    spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a0e2ce289d7bb5fbcfe7e436c3c24b3a6">spline_x</a> = spline_x;</div><div class="line"><a name="l00094"></a><span class="lineno">   94</span>&#160;    spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a149445d98f325cd01096a829e4ac90d8">spline_y</a> = spline_y;</div><div class="line"><a name="l00095"></a><span class="lineno">   95</span>&#160;    spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a60e87b9c4f18726568ef29bb43fba580">spline_z</a> = spline_z;</div><div class="line"><a name="l00096"></a><span class="lineno">   96</span>&#160;    spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a372cfda50d7d422fa234a2f8ebec21ee">is_valid</a> = is_ok_x and is_ok_y and is_ok_z;</div><div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;    spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>.assign(knots.data(),knots.data()+knots.size()) ;</div><div class="line"><a name="l00098"></a><span class="lineno">   98</span>&#160;    spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a1315136ffc90e4164123f07aef230a6e">n_seg</a> = spline_x.n_seg;</div><div class="line"><a name="l00099"></a><span class="lineno">   99</span>&#160;    spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a868db24a21948436dd8cbe7c71241bc1">poly_order</a> = poly_order;</div><div class="line"><a name="l00100"></a><span class="lineno">  100</span>&#160;</div><div class="line"><a name="l00101"></a><span class="lineno">  101</span>&#160;    <span class="comment">// update current path </span></div><div class="line"><a name="l00102"></a><span class="lineno">  102</span>&#160;    <a class="code" href="class_path_planner.html#ad231acc2bce7d5e143fa49c69515c4a9">horizon_eval_spline</a>(10);</div><div class="line"><a name="l00103"></a><span class="lineno">  103</span>&#160;</div><div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;}</div><div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;</div><div class="line"><a name="l00106"></a><span class="lineno">  106</span>&#160;</div><div class="line"><a name="l00107"></a><span class="lineno"><a class="line" href="class_path_planner.html#a4b5f5beea2c380ae4340f65dd1d5bd10">  107</a></span>&#160;<a class="code" href="struct_q_p__form__xyz.html">QP_form_xyz</a> <a class="code" href="class_path_planner.html#a4b5f5beea2c380ae4340f65dd1d5bd10">PathPlanner::qp_gen</a>(<span class="keyword">const</span> <a class="code" href="_poly_traj_gen_8h.html#a37d0493e73d8ef72a2405596ad6cee0c">TimeSeries</a>&amp; knots,<span class="keyword">const</span> nav_msgs::Path&amp; waypoints,<span class="keyword">const</span> geometry_msgs::Twist&amp; v0,<span class="keyword">const</span> geometry_msgs::Twist&amp; a0,<a class="code" href="struct_traj_gen_opts.html">TrajGenOpts</a> opt ){</div><div class="line"><a name="l00108"></a><span class="lineno">  108</span>&#160;</div><div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;</div><div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;</div><div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;    <span class="keywordtype">int</span> n_seg = waypoints.poses.size() - 1;</div><div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;    <span class="keywordtype">int</span> poly_order = opt.<a class="code" href="struct_traj_gen_opts.html#a538985230ef4a8cd7763c91d11bebbaf">poly_order</a>;</div><div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;    <span class="comment">// this should be changed when we adopt single box </span></div><div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;    <span class="keywordtype">int</span> n_var_total = (poly_order + 1) * n_seg; <span class="comment">// in case of the seperable x,y,z optimization </span></div><div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;    <span class="keywordtype">int</span> blck_size=poly_order+1;</div><div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;</div><div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;    MatrixXd Q(n_var_total,n_var_total),H(1,n_var_total);</div><div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;    Q.setZero(); H.setZero();</div><div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;    MatrixXd Qx = Q, Qy = Q, Qz = Q; </div><div class="line"><a name="l00120"></a><span class="lineno">  120</span>&#160;    MatrixXd Hx = H, Hy = H, Hz = H;</div><div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;</div><div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;    MatrixXd Aeq(0,n_var_total),beq(0,1),Aineq(0,n_var_total),bineq(0,1);</div><div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;    Aeq.setZero(); beq.setZero(); Aineq.setZero(); bineq.setZero();</div><div class="line"><a name="l00124"></a><span class="lineno">  124</span>&#160;    MatrixXd Aeq_x = Aeq, Aeq_y = Aeq, Aeq_z =Aeq;</div><div class="line"><a name="l00125"></a><span class="lineno">  125</span>&#160;    MatrixXd beq_x = beq, beq_y = beq, beq_z =beq;  </div><div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;    MatrixXd Aineq_x = Aineq, Aineq_y = Aineq, Aineq_z =Aineq;</div><div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;    MatrixXd bineq_x = bineq, bineq_y = bineq, bineq_z =bineq;  </div><div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;    </div><div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;</div><div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;    <span class="comment">/*</span></div><div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;<span class="comment">        1. Cost funtion </span></div><div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;<span class="comment">    */</span></div><div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;    <span class="comment">//  -----------------------------------------------------------------------</span></div><div class="line"><a name="l00134"></a><span class="lineno">  134</span>&#160;</div><div class="line"><a name="l00135"></a><span class="lineno">  135</span>&#160;    <span class="comment">// if minimum jerk </span></div><div class="line"><a name="l00136"></a><span class="lineno">  136</span>&#160;    <span class="keywordflow">if</span> (opt.<a class="code" href="struct_traj_gen_opts.html#ad8937f645e1154dc52978bd6592ce836">objective_derivative</a> == 3){    </div><div class="line"><a name="l00137"></a><span class="lineno">  137</span>&#160;        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> n = 0; n &lt; n_seg; n++) {</div><div class="line"><a name="l00138"></a><span class="lineno">  138</span>&#160;            MatrixXd Dn = <a class="code" href="_poly_traj_gen_8cpp.html#a5a2e38cc8bf6ae9ab0395994b68e52ae">time_scailing_mat</a>(knots[n + 1]-knots[n], poly_order);</div><div class="line"><a name="l00139"></a><span class="lineno">  139</span>&#160;            <span class="keywordtype">double</span> dn = knots[n + 1] - knots[n];</div><div class="line"><a name="l00140"></a><span class="lineno">  140</span>&#160;            <span class="comment">// Q.block(blck_size*(n),blck_size*(n),blck_size,blck_size)=Dn*integral_jerk_squared(poly_order)*Dn/pow(dn,5);</span></div><div class="line"><a name="l00141"></a><span class="lineno">  141</span>&#160;            Q.block(blck_size*(n),blck_size*(n),blck_size,blck_size)=<a class="code" href="_poly_traj_gen_8cpp.html#a0b792e87b38c7882b087726a82eaa72e">integral_jerk_squared</a>(poly_order);</div><div class="line"><a name="l00142"></a><span class="lineno">  142</span>&#160;        }         </div><div class="line"><a name="l00143"></a><span class="lineno">  143</span>&#160;    }</div><div class="line"><a name="l00144"></a><span class="lineno">  144</span>&#160;    <span class="comment">// if minimum snap </span></div><div class="line"><a name="l00145"></a><span class="lineno">  145</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span>(opt.<a class="code" href="struct_traj_gen_opts.html#ad8937f645e1154dc52978bd6592ce836">objective_derivative</a> == 4){</div><div class="line"><a name="l00146"></a><span class="lineno">  146</span>&#160;        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> n = 0; n &lt; n_seg; n++) {</div><div class="line"><a name="l00147"></a><span class="lineno">  147</span>&#160;            MatrixXd Dn = <a class="code" href="_poly_traj_gen_8cpp.html#a5a2e38cc8bf6ae9ab0395994b68e52ae">time_scailing_mat</a>(knots[n + 1]-knots[n], poly_order);</div><div class="line"><a name="l00148"></a><span class="lineno">  148</span>&#160;            <span class="keywordtype">double</span> dn = knots[n + 1] - knots[n];</div><div class="line"><a name="l00149"></a><span class="lineno">  149</span>&#160;            <span class="comment">// Q.block(blck_size*(n),blck_size*(n),blck_size,blck_size)=Dn*integral_snap_squared(poly_order)*Dn/pow(dn,7);</span></div><div class="line"><a name="l00150"></a><span class="lineno">  150</span>&#160;            Q.block(blck_size*(n),blck_size*(n),blck_size,blck_size)=<a class="code" href="_poly_traj_gen_8cpp.html#a98c8e48d65c22d6da6b6b82242586f15">integral_snap_squared</a>(poly_order);</div><div class="line"><a name="l00151"></a><span class="lineno">  151</span>&#160;</div><div class="line"><a name="l00152"></a><span class="lineno">  152</span>&#160;        }</div><div class="line"><a name="l00153"></a><span class="lineno">  153</span>&#160;    }<span class="keywordflow">else</span>{</div><div class="line"><a name="l00154"></a><span class="lineno">  154</span>&#160;        cerr&lt;&lt;<span class="stringliteral">&quot;undefined derivative in objective&quot;</span>&lt;&lt;endl;</div><div class="line"><a name="l00155"></a><span class="lineno">  155</span>&#160;        <span class="keywordflow">return</span> <a class="code" href="struct_q_p__form__xyz.html">QP_form_xyz</a>();</div><div class="line"><a name="l00156"></a><span class="lineno">  156</span>&#160;    }</div><div class="line"><a name="l00157"></a><span class="lineno">  157</span>&#160;</div><div class="line"><a name="l00158"></a><span class="lineno">  158</span>&#160;    <span class="comment">// if it is soft, we include the deviation term </span></div><div class="line"><a name="l00159"></a><span class="lineno">  159</span>&#160;    <span class="keywordflow">if</span> (opt.<a class="code" href="struct_traj_gen_opts.html#a5f27ab3f06720accfeaf88d44da2c5d6">is_waypoint_soft</a>){    </div><div class="line"><a name="l00160"></a><span class="lineno">  160</span>&#160;            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> n=0;n&lt;n_seg;n++){</div><div class="line"><a name="l00161"></a><span class="lineno">  161</span>&#160;                MatrixXd Dn = <a class="code" href="_poly_traj_gen_8cpp.html#a5a2e38cc8bf6ae9ab0395994b68e52ae">time_scailing_mat</a>(knots[n + 1]-knots[n], poly_order);</div><div class="line"><a name="l00162"></a><span class="lineno">  162</span>&#160;                <span class="keywordtype">int</span> insert_start=blck_size*(n);</div><div class="line"><a name="l00163"></a><span class="lineno">  163</span>&#160;                <span class="keywordtype">double</span> time_scaling_factor;</div><div class="line"><a name="l00164"></a><span class="lineno">  164</span>&#160;                <span class="keywordflow">if</span> (opt.<a class="code" href="struct_traj_gen_opts.html#ad8937f645e1154dc52978bd6592ce836">objective_derivative</a> == 3)</div><div class="line"><a name="l00165"></a><span class="lineno">  165</span>&#160;                    time_scaling_factor = pow(knots[n+1]-knots[n],5);</div><div class="line"><a name="l00166"></a><span class="lineno">  166</span>&#160;                <span class="keywordflow">else</span> </div><div class="line"><a name="l00167"></a><span class="lineno">  167</span>&#160;                    time_scaling_factor = pow(knots[n+1]-knots[n],7);</div><div class="line"><a name="l00168"></a><span class="lineno">  168</span>&#160;</div><div class="line"><a name="l00169"></a><span class="lineno">  169</span>&#160;                cout&lt;&lt;<span class="stringliteral">&quot;time scaling factor: &quot;</span>&lt;&lt;time_scaling_factor&lt;&lt;endl;</div><div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;                Q.block(insert_start,insert_start,blck_size,blck_size)+=time_scaling_factor*opt.<a class="code" href="struct_traj_gen_opts.html#a7424679070ea7264f7c8b6df5461bdb9">w_d</a>*<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,1,0)*<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,1,0).transpose();</div><div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;                Hx.block(0,insert_start,1,blck_size)=-2*time_scaling_factor*opt.<a class="code" href="struct_traj_gen_opts.html#a7424679070ea7264f7c8b6df5461bdb9">w_d</a>*(waypoints.poses[n+1].pose.position.x)*<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,1,0).transpose();</div><div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;                Hy.block(0,insert_start,1,blck_size)=-2*time_scaling_factor*opt.<a class="code" href="struct_traj_gen_opts.html#a7424679070ea7264f7c8b6df5461bdb9">w_d</a>*(waypoints.poses[n+1].pose.position.y)*<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,1,0).transpose();</div><div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;                Hz.block(0,insert_start,1,blck_size)=-2*time_scaling_factor*opt.<a class="code" href="struct_traj_gen_opts.html#a7424679070ea7264f7c8b6df5461bdb9">w_d</a>*(waypoints.poses[n+1].pose.position.z)*<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,1,0).transpose();</div><div class="line"><a name="l00174"></a><span class="lineno">  174</span>&#160;            }</div><div class="line"><a name="l00175"></a><span class="lineno">  175</span>&#160;    </div><div class="line"><a name="l00176"></a><span class="lineno">  176</span>&#160;    }</div><div class="line"><a name="l00177"></a><span class="lineno">  177</span>&#160;    Qx = Q; Qy = Q; Qz = Q;</div><div class="line"><a name="l00178"></a><span class="lineno">  178</span>&#160;</div><div class="line"><a name="l00179"></a><span class="lineno">  179</span>&#160;</div><div class="line"><a name="l00180"></a><span class="lineno">  180</span>&#160;</div><div class="line"><a name="l00181"></a><span class="lineno">  181</span>&#160;    <span class="comment">/*</span></div><div class="line"><a name="l00182"></a><span class="lineno">  182</span>&#160;<span class="comment">        2. Equality constraints  </span></div><div class="line"><a name="l00183"></a><span class="lineno">  183</span>&#160;<span class="comment">    */</span></div><div class="line"><a name="l00184"></a><span class="lineno">  184</span>&#160;    <span class="comment">//  -----------------------------------------------------------------------</span></div><div class="line"><a name="l00185"></a><span class="lineno">  185</span>&#160;</div><div class="line"><a name="l00186"></a><span class="lineno">  186</span>&#160;    <span class="comment">// (1) Initial constraints </span></div><div class="line"><a name="l00187"></a><span class="lineno">  187</span>&#160;</div><div class="line"><a name="l00188"></a><span class="lineno">  188</span>&#160;    MatrixXd Aeq0(3,n_var_total),beq0_sub(3,1);</div><div class="line"><a name="l00189"></a><span class="lineno">  189</span>&#160;    Aeq0.setZero(), beq0_sub.setZero();</div><div class="line"><a name="l00190"></a><span class="lineno">  190</span>&#160;    Aeq0.block(0,0,3,poly_order+1)  = <a class="code" href="class_path_planner.html#a1d6a959095513585bfc10468f7b8d180">get_init_constraint_mat</a>(waypoints.poses[0].pose.position.x,v0.linear.x,a0.linear.x,opt).<a class="code" href="struct_constraint.html#a95639f38cb8a18c60a96e629160a9571">A</a>;</div><div class="line"><a name="l00191"></a><span class="lineno">  191</span>&#160;    <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(Aeq_x,Aeq0); </div><div class="line"><a name="l00192"></a><span class="lineno">  192</span>&#160;    <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(Aeq_y,Aeq0); </div><div class="line"><a name="l00193"></a><span class="lineno">  193</span>&#160;    <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(Aeq_z,Aeq0);         </div><div class="line"><a name="l00194"></a><span class="lineno">  194</span>&#160;</div><div class="line"><a name="l00195"></a><span class="lineno">  195</span>&#160;    beq0_sub.block(0,0,3,1)  = <a class="code" href="class_path_planner.html#a1d6a959095513585bfc10468f7b8d180">get_init_constraint_mat</a>(waypoints.poses[0].pose.position.x,v0.linear.x,a0.linear.x,opt).<a class="code" href="struct_constraint.html#ab22b0463922cee8c89b0443ed611d072">b</a>;     </div><div class="line"><a name="l00196"></a><span class="lineno">  196</span>&#160;    <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(beq_x,beq0_sub);</div><div class="line"><a name="l00197"></a><span class="lineno">  197</span>&#160;    beq0_sub.block(0,0,3,1)  = <a class="code" href="class_path_planner.html#a1d6a959095513585bfc10468f7b8d180">get_init_constraint_mat</a>(waypoints.poses[0].pose.position.y,v0.linear.y,a0.linear.y,opt).<a class="code" href="struct_constraint.html#ab22b0463922cee8c89b0443ed611d072">b</a>;  </div><div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;    <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(beq_y,beq0_sub);</div><div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;    beq0_sub.block(0,0,3,1)  = <a class="code" href="class_path_planner.html#a1d6a959095513585bfc10468f7b8d180">get_init_constraint_mat</a>(waypoints.poses[0].pose.position.z,v0.linear.z,a0.linear.z,opt).<a class="code" href="struct_constraint.html#ab22b0463922cee8c89b0443ed611d072">b</a>;     </div><div class="line"><a name="l00200"></a><span class="lineno">  200</span>&#160;    <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(beq_z,beq0_sub);    </div><div class="line"><a name="l00201"></a><span class="lineno">  201</span>&#160;   </div><div class="line"><a name="l00202"></a><span class="lineno">  202</span>&#160;    <span class="comment">// (2) Waypoints constraints (if it is hard constrained)</span></div><div class="line"><a name="l00203"></a><span class="lineno">  203</span>&#160;</div><div class="line"><a name="l00204"></a><span class="lineno">  204</span>&#160;    <span class="keywordflow">if</span>(not opt.<a class="code" href="struct_traj_gen_opts.html#a5f27ab3f06720accfeaf88d44da2c5d6">is_waypoint_soft</a>)</div><div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> k = 0;k&lt;n_seg;k++){            </div><div class="line"><a name="l00206"></a><span class="lineno">  206</span>&#160;            <span class="keywordtype">int</span> insert_idx = k*blck_size;</div><div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160;            MatrixXd Dn = <a class="code" href="_poly_traj_gen_8cpp.html#a5a2e38cc8bf6ae9ab0395994b68e52ae">time_scailing_mat</a>(knots[k + 1]-knots[k], poly_order);</div><div class="line"><a name="l00208"></a><span class="lineno">  208</span>&#160;            MatrixXd Aeq_sub(1,n_var_total),beq_sub(1,1);</div><div class="line"><a name="l00209"></a><span class="lineno">  209</span>&#160;            Aeq_sub.setZero(); beq_sub.setZero();</div><div class="line"><a name="l00210"></a><span class="lineno">  210</span>&#160;            Aeq_sub.block(0,insert_idx,1,blck_size) = <a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,1,0).transpose();</div><div class="line"><a name="l00211"></a><span class="lineno">  211</span>&#160;            <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(Aeq_x,Aeq_sub); </div><div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;            <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(Aeq_y,Aeq_sub); </div><div class="line"><a name="l00213"></a><span class="lineno">  213</span>&#160;            <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(Aeq_z,Aeq_sub);             </div><div class="line"><a name="l00214"></a><span class="lineno">  214</span>&#160;            </div><div class="line"><a name="l00215"></a><span class="lineno">  215</span>&#160;            beq_sub(0) = waypoints.poses[k+1].pose.position.x;</div><div class="line"><a name="l00216"></a><span class="lineno">  216</span>&#160;            <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(beq_x,beq_sub);</div><div class="line"><a name="l00217"></a><span class="lineno">  217</span>&#160;            beq_sub(0) = waypoints.poses[k+1].pose.position.y;</div><div class="line"><a name="l00218"></a><span class="lineno">  218</span>&#160;            <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(beq_y,beq_sub);</div><div class="line"><a name="l00219"></a><span class="lineno">  219</span>&#160;            beq_sub(0) = waypoints.poses[k+1].pose.position.z;</div><div class="line"><a name="l00220"></a><span class="lineno">  220</span>&#160;            <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(beq_z,beq_sub);</div><div class="line"><a name="l00221"></a><span class="lineno">  221</span>&#160;            </div><div class="line"><a name="l00222"></a><span class="lineno">  222</span>&#160;        }</div><div class="line"><a name="l00223"></a><span class="lineno">  223</span>&#160;</div><div class="line"><a name="l00224"></a><span class="lineno">  224</span>&#160;</div><div class="line"><a name="l00225"></a><span class="lineno">  225</span>&#160;    <span class="comment">// (3) continuity constraints     </span></div><div class="line"><a name="l00226"></a><span class="lineno">  226</span>&#160;    <span class="comment">// if the objective is snap, we will include 3rd order continuity </span></div><div class="line"><a name="l00227"></a><span class="lineno">  227</span>&#160;    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> k = 0;k&lt;n_seg-1;k++){</div><div class="line"><a name="l00228"></a><span class="lineno">  228</span>&#160;        <span class="keywordtype">int</span> insert_idx = k*blck_size;        </div><div class="line"><a name="l00229"></a><span class="lineno">  229</span>&#160;        MatrixXd Aeq_sub(opt.<a class="code" href="struct_traj_gen_opts.html#ad8937f645e1154dc52978bd6592ce836">objective_derivative</a>,n_var_total),beq_sub(opt.<a class="code" href="struct_traj_gen_opts.html#ad8937f645e1154dc52978bd6592ce836">objective_derivative</a>,1);</div><div class="line"><a name="l00230"></a><span class="lineno">  230</span>&#160;        Aeq_sub.setZero(); beq_sub.setZero();</div><div class="line"><a name="l00231"></a><span class="lineno">  231</span>&#160;        Aeq_sub.block(0,insert_idx,opt.<a class="code" href="struct_traj_gen_opts.html#ad8937f645e1154dc52978bd6592ce836">objective_derivative</a>,2*blck_size) = <a class="code" href="class_path_planner.html#aff99e738bd54babad38867d7713e5a51">get_continuity_constraint_mat</a>(knots[k+1]-knots[k],knots[k+2]-knots[k+1],opt).<a class="code" href="struct_constraint.html#a95639f38cb8a18c60a96e629160a9571">A</a>;</div><div class="line"><a name="l00232"></a><span class="lineno">  232</span>&#160;        beq_sub = <a class="code" href="class_path_planner.html#aff99e738bd54babad38867d7713e5a51">get_continuity_constraint_mat</a>(knots[k+1]-knots[k],knots[k+2]-knots[k+1],opt).<a class="code" href="struct_constraint.html#ab22b0463922cee8c89b0443ed611d072">b</a>;</div><div class="line"><a name="l00233"></a><span class="lineno">  233</span>&#160;        <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(Aeq_x,Aeq_sub); <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(beq_x,beq_sub);</div><div class="line"><a name="l00234"></a><span class="lineno">  234</span>&#160;        <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(Aeq_y,Aeq_sub); <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(beq_y,beq_sub);</div><div class="line"><a name="l00235"></a><span class="lineno">  235</span>&#160;        <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(Aeq_z,Aeq_sub); <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(beq_z,beq_sub);              </div><div class="line"><a name="l00236"></a><span class="lineno">  236</span>&#160;    }</div><div class="line"><a name="l00237"></a><span class="lineno">  237</span>&#160;</div><div class="line"><a name="l00238"></a><span class="lineno">  238</span>&#160;</div><div class="line"><a name="l00239"></a><span class="lineno">  239</span>&#160;    <span class="keywordtype">double</span> safe_r = opt.<a class="code" href="struct_traj_gen_opts.html#aba857cb0ae86cbac99ba5adf9320c66d">safe_r</a>;</div><div class="line"><a name="l00240"></a><span class="lineno">  240</span>&#160;    <span class="keywordtype">int</span> N_safe_pnts = opt.<a class="code" href="struct_traj_gen_opts.html#a216fe9912362cc5e876f945112811c35">N_safe_pnts</a>;</div><div class="line"><a name="l00241"></a><span class="lineno">  241</span>&#160;    <span class="keywordtype">int</span> n_ineq_consts = 2*(N_safe_pnts)*n_seg;</div><div class="line"><a name="l00242"></a><span class="lineno">  242</span>&#160;</div><div class="line"><a name="l00243"></a><span class="lineno">  243</span>&#160;</div><div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;    <span class="comment">/*</span></div><div class="line"><a name="l00245"></a><span class="lineno">  245</span>&#160;<span class="comment">        3. Inequality constraints (only if there is a corridor constraint) </span></div><div class="line"><a name="l00246"></a><span class="lineno">  246</span>&#160;<span class="comment">    */</span></div><div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;    <span class="comment">//  -----------------------------------------------------------------------    </span></div><div class="line"><a name="l00248"></a><span class="lineno">  248</span>&#160;</div><div class="line"><a name="l00249"></a><span class="lineno">  249</span>&#160;    <span class="keywordflow">if</span> (opt.<a class="code" href="struct_traj_gen_opts.html#a7ba33a57327fba375f137f1f0104c1ea">is_multi_corridor</a> or opt.<a class="code" href="struct_traj_gen_opts.html#ad4725655d38467206ed000eacfd6c54a">is_single_corridor</a>){    </div><div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;</div><div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;        <span class="comment">// in case of multi corridor, the multiple cubes are used to represent the corridor region </span></div><div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;        <span class="keywordflow">if</span>(opt.<a class="code" href="struct_traj_gen_opts.html#a7ba33a57327fba375f137f1f0104c1ea">is_multi_corridor</a>){</div><div class="line"><a name="l00253"></a><span class="lineno">  253</span>&#160;            <span class="comment">// multi-corridor is called. if previous solve routine was single corridor, flush it </span></div><div class="line"><a name="l00254"></a><span class="lineno">  254</span>&#160;            <span class="keywordflow">if</span>(safe_corridor_marker_single_array.markers.size())</div><div class="line"><a name="l00255"></a><span class="lineno">  255</span>&#160;                safe_corridor_marker_single_array.markers.clear(); </div><div class="line"><a name="l00256"></a><span class="lineno">  256</span>&#160;</div><div class="line"><a name="l00257"></a><span class="lineno">  257</span>&#160;            safe_corridor_marker.header.frame_id = <span class="stringliteral">&quot;/world&quot;</span>;</div><div class="line"><a name="l00258"></a><span class="lineno">  258</span>&#160;            safe_corridor_marker.ns = <span class="stringliteral">&quot;sf_corridor&quot;</span>;</div><div class="line"><a name="l00259"></a><span class="lineno">  259</span>&#160;            safe_corridor_marker.type = 6; <span class="comment">// cube list </span></div><div class="line"><a name="l00260"></a><span class="lineno">  260</span>&#160;            safe_corridor_marker.scale.x = 2*safe_r;</div><div class="line"><a name="l00261"></a><span class="lineno">  261</span>&#160;            safe_corridor_marker.scale.y = 2*safe_r;</div><div class="line"><a name="l00262"></a><span class="lineno">  262</span>&#160;            safe_corridor_marker.scale.z = 2*safe_r;</div><div class="line"><a name="l00263"></a><span class="lineno">  263</span>&#160;            safe_corridor_marker.action = 0;</div><div class="line"><a name="l00264"></a><span class="lineno">  264</span>&#160;            safe_corridor_marker.pose.orientation.w = 1;</div><div class="line"><a name="l00265"></a><span class="lineno">  265</span>&#160;            safe_corridor_marker.color.a = 0.5;</div><div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;            safe_corridor_marker.color.r = 170.0/255.0;</div><div class="line"><a name="l00267"></a><span class="lineno">  267</span>&#160;            safe_corridor_marker.color.g = 1.0;</div><div class="line"><a name="l00268"></a><span class="lineno">  268</span>&#160;            safe_corridor_marker.color.b = 1.0;</div><div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160;            safe_corridor_marker.points.clear();</div><div class="line"><a name="l00270"></a><span class="lineno">  270</span>&#160;            safe_corridor_marker.points.resize((N_safe_pnts)*n_seg);</div><div class="line"><a name="l00271"></a><span class="lineno">  271</span>&#160;        }</div><div class="line"><a name="l00272"></a><span class="lineno">  272</span>&#160;        <span class="comment">// in the single corridor, affine transform is needed </span></div><div class="line"><a name="l00273"></a><span class="lineno">  273</span>&#160;        <span class="keywordflow">else</span> <span class="keywordflow">if</span> (opt.<a class="code" href="struct_traj_gen_opts.html#ad4725655d38467206ed000eacfd6c54a">is_single_corridor</a>){</div><div class="line"><a name="l00274"></a><span class="lineno">  274</span>&#160;            <span class="keywordflow">if</span>(safe_corridor_marker.points.size())</div><div class="line"><a name="l00275"></a><span class="lineno">  275</span>&#160;                safe_corridor_marker.points.clear();</div><div class="line"><a name="l00276"></a><span class="lineno">  276</span>&#160;</div><div class="line"><a name="l00277"></a><span class="lineno">  277</span>&#160;            safe_corridor_marker_single_base.header.frame_id = <span class="stringliteral">&quot;world&quot;</span>;</div><div class="line"><a name="l00278"></a><span class="lineno">  278</span>&#160;            safe_corridor_marker_single_base.ns = <span class="stringliteral">&quot;sf_corridor&quot;</span>;</div><div class="line"><a name="l00279"></a><span class="lineno">  279</span>&#160;            safe_corridor_marker_single_base.type = visualization_msgs::Marker::CUBE;</div><div class="line"><a name="l00280"></a><span class="lineno">  280</span>&#160;            safe_corridor_marker_single_base.action = 0;</div><div class="line"><a name="l00281"></a><span class="lineno">  281</span>&#160;            safe_corridor_marker_single_base.color.a = 0.5;</div><div class="line"><a name="l00282"></a><span class="lineno">  282</span>&#160;            safe_corridor_marker_single_base.color.r = 170.0/255.0;</div><div class="line"><a name="l00283"></a><span class="lineno">  283</span>&#160;            safe_corridor_marker_single_base.color.g = 1.0;</div><div class="line"><a name="l00284"></a><span class="lineno">  284</span>&#160;            safe_corridor_marker_single_base.color.b = 1.0;</div><div class="line"><a name="l00285"></a><span class="lineno">  285</span>&#160;            </div><div class="line"><a name="l00286"></a><span class="lineno">  286</span>&#160;            <span class="comment">// flushing </span></div><div class="line"><a name="l00287"></a><span class="lineno">  287</span>&#160;</div><div class="line"><a name="l00288"></a><span class="lineno">  288</span>&#160;            <span class="keywordflow">if</span>(safe_corridor_marker_single_array.markers.size())</div><div class="line"><a name="l00289"></a><span class="lineno">  289</span>&#160;                safe_corridor_marker_single_array.markers.clear();</div><div class="line"><a name="l00290"></a><span class="lineno">  290</span>&#160;        }           </div><div class="line"><a name="l00291"></a><span class="lineno">  291</span>&#160;        </div><div class="line"><a name="l00292"></a><span class="lineno">  292</span>&#160;</div><div class="line"><a name="l00293"></a><span class="lineno">  293</span>&#160;        MatrixXd A_sub(n_ineq_consts,n_var_total),bx_sub(n_ineq_consts,1),by_sub(n_ineq_consts,1),bz_sub(n_ineq_consts,1);</div><div class="line"><a name="l00294"></a><span class="lineno">  294</span>&#160;        A_sub.setZero(); bx_sub.setZero(); by_sub.setZero(); bz_sub.setZero();</div><div class="line"><a name="l00295"></a><span class="lineno">  295</span>&#160;        </div><div class="line"><a name="l00296"></a><span class="lineno">  296</span>&#160;        <span class="keywordtype">int</span> ineq_col_insert_idx1=0,ineq_row_insert_idx = 0; </div><div class="line"><a name="l00297"></a><span class="lineno">  297</span>&#160;        <span class="keywordtype">int</span> idx = 0;</div><div class="line"><a name="l00298"></a><span class="lineno">  298</span>&#160;</div><div class="line"><a name="l00299"></a><span class="lineno">  299</span>&#160;        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> n = 0 ; n&lt;n_seg;n++){</div><div class="line"><a name="l00300"></a><span class="lineno">  300</span>&#160;</div><div class="line"><a name="l00301"></a><span class="lineno">  301</span>&#160;            <span class="comment">// sub points along this line segment </span></div><div class="line"><a name="l00302"></a><span class="lineno">  302</span>&#160;            <span class="keywordtype">double</span> x0,y0,z0;</div><div class="line"><a name="l00303"></a><span class="lineno">  303</span>&#160;            <span class="keywordtype">double</span> xf,yf,zf;</div><div class="line"><a name="l00304"></a><span class="lineno">  304</span>&#160;            <span class="keywordtype">double</span> dx,dy,dz;</div><div class="line"><a name="l00305"></a><span class="lineno">  305</span>&#160;            </div><div class="line"><a name="l00306"></a><span class="lineno">  306</span>&#160;            x0 = waypoints.poses[n].pose.position.x;</div><div class="line"><a name="l00307"></a><span class="lineno">  307</span>&#160;            y0 = waypoints.poses[n].pose.position.y;</div><div class="line"><a name="l00308"></a><span class="lineno">  308</span>&#160;            z0 = waypoints.poses[n].pose.position.z;</div><div class="line"><a name="l00309"></a><span class="lineno">  309</span>&#160;</div><div class="line"><a name="l00310"></a><span class="lineno">  310</span>&#160;            xf = waypoints.poses[n+1].pose.position.x;</div><div class="line"><a name="l00311"></a><span class="lineno">  311</span>&#160;            yf = waypoints.poses[n+1].pose.position.y;</div><div class="line"><a name="l00312"></a><span class="lineno">  312</span>&#160;            zf = waypoints.poses[n+1].pose.position.z;</div><div class="line"><a name="l00313"></a><span class="lineno">  313</span>&#160;            </div><div class="line"><a name="l00314"></a><span class="lineno">  314</span>&#160;            dx = (xf-x0)/((N_safe_pnts+1));</div><div class="line"><a name="l00315"></a><span class="lineno">  315</span>&#160;            dy = (yf-y0)/((N_safe_pnts+1));</div><div class="line"><a name="l00316"></a><span class="lineno">  316</span>&#160;            dz = (zf-z0)/((N_safe_pnts+1));</div><div class="line"><a name="l00317"></a><span class="lineno">  317</span>&#160;            <span class="keywordflow">for</span> (<span class="keywordtype">int</span> n_sub = 1; n_sub&lt;=N_safe_pnts;n_sub++){</div><div class="line"><a name="l00318"></a><span class="lineno">  318</span>&#160;</div><div class="line"><a name="l00319"></a><span class="lineno">  319</span>&#160;                <span class="keywordtype">double</span> x_sub,y_sub,z_sub;</div><div class="line"><a name="l00320"></a><span class="lineno">  320</span>&#160;                x_sub = x0 + dx*n_sub;</div><div class="line"><a name="l00321"></a><span class="lineno">  321</span>&#160;                y_sub = y0 + dy*n_sub;</div><div class="line"><a name="l00322"></a><span class="lineno">  322</span>&#160;                z_sub = z0 + dz*n_sub;</div><div class="line"><a name="l00323"></a><span class="lineno">  323</span>&#160;                </div><div class="line"><a name="l00324"></a><span class="lineno">  324</span>&#160;               <span class="keywordtype">double</span> t_control = 1.0/(N_safe_pnts+1) * n_sub;</div><div class="line"><a name="l00325"></a><span class="lineno">  325</span>&#160;</div><div class="line"><a name="l00326"></a><span class="lineno">  326</span>&#160;                </div><div class="line"><a name="l00327"></a><span class="lineno">  327</span>&#160;                Vector3f upper_limit,lower_limit;</div><div class="line"><a name="l00328"></a><span class="lineno">  328</span>&#160;</div><div class="line"><a name="l00329"></a><span class="lineno">  329</span>&#160;                <span class="comment">// axis parallel multiple cube</span></div><div class="line"><a name="l00330"></a><span class="lineno">  330</span>&#160;                <span class="keywordflow">if</span> (opt.<a class="code" href="struct_traj_gen_opts.html#a7ba33a57327fba375f137f1f0104c1ea">is_multi_corridor</a>){</div><div class="line"><a name="l00331"></a><span class="lineno">  331</span>&#160;                    lower_limit(0) = -(x_sub - safe_r);</div><div class="line"><a name="l00332"></a><span class="lineno">  332</span>&#160;                    lower_limit(1) = -(y_sub - safe_r);</div><div class="line"><a name="l00333"></a><span class="lineno">  333</span>&#160;                    lower_limit(2) = -(z_sub - safe_r);</div><div class="line"><a name="l00334"></a><span class="lineno">  334</span>&#160;                     </div><div class="line"><a name="l00335"></a><span class="lineno">  335</span>&#160;                    upper_limit(0) = (x_sub + safe_r);</div><div class="line"><a name="l00336"></a><span class="lineno">  336</span>&#160;                    upper_limit(1) = (y_sub + safe_r);</div><div class="line"><a name="l00337"></a><span class="lineno">  337</span>&#160;                    upper_limit(2) = (z_sub + safe_r);</div><div class="line"><a name="l00338"></a><span class="lineno">  338</span>&#160;</div><div class="line"><a name="l00339"></a><span class="lineno">  339</span>&#160;                    </div><div class="line"><a name="l00340"></a><span class="lineno">  340</span>&#160;</div><div class="line"><a name="l00341"></a><span class="lineno">  341</span>&#160;                    <span class="comment">// marker update               </span></div><div class="line"><a name="l00342"></a><span class="lineno">  342</span>&#160;                    safe_corridor_marker.points[idx].x = x_sub;</div><div class="line"><a name="l00343"></a><span class="lineno">  343</span>&#160;                    safe_corridor_marker.points[idx].y = y_sub;</div><div class="line"><a name="l00344"></a><span class="lineno">  344</span>&#160;                    safe_corridor_marker.points[idx].z = z_sub;</div><div class="line"><a name="l00345"></a><span class="lineno">  345</span>&#160;                    idx++;</div><div class="line"><a name="l00346"></a><span class="lineno">  346</span>&#160;</div><div class="line"><a name="l00347"></a><span class="lineno">  347</span>&#160;                <span class="comment">// non parallel single rectangle</span></div><div class="line"><a name="l00348"></a><span class="lineno">  348</span>&#160;                }<span class="keywordflow">else</span> <span class="keywordflow">if</span>(opt.<a class="code" href="struct_traj_gen_opts.html#ad4725655d38467206ed000eacfd6c54a">is_single_corridor</a>){</div><div class="line"><a name="l00349"></a><span class="lineno">  349</span>&#160;                    <span class="comment">//  </span></div><div class="line"><a name="l00350"></a><span class="lineno">  350</span>&#160;                    Point p1,p2;</div><div class="line"><a name="l00351"></a><span class="lineno">  351</span>&#160;                    p1.x = x0; p1.y = y0; p1.z = z0;</div><div class="line"><a name="l00352"></a><span class="lineno">  352</span>&#160;                    p2.x = xf; p2.y = yf; p2.z = zf;</div><div class="line"><a name="l00353"></a><span class="lineno">  353</span>&#160;                    Vector3f pnt1(p1.x,p1.y,p1.z),pnt2(p2.x,p2.y,p2.z);  </div><div class="line"><a name="l00354"></a><span class="lineno">  354</span>&#160;                    <span class="keywordtype">float</span> l = (pnt1 - pnt2).norm();</div><div class="line"><a name="l00355"></a><span class="lineno">  355</span>&#160;                    Affine3f Twb = get_affine_corridor(p1,p2);</div><div class="line"><a name="l00356"></a><span class="lineno">  356</span>&#160;                    </div><div class="line"><a name="l00357"></a><span class="lineno">  357</span>&#160;                    cout&lt;&lt;<span class="stringliteral">&quot;rotation&quot;</span>&lt;&lt;endl;</div><div class="line"><a name="l00358"></a><span class="lineno">  358</span>&#160;                    cout&lt;&lt;Twb.rotation()&lt;&lt;std::endl;</div><div class="line"><a name="l00359"></a><span class="lineno">  359</span>&#160;</div><div class="line"><a name="l00360"></a><span class="lineno">  360</span>&#160;                    cout&lt;&lt;<span class="stringliteral">&quot;translation&quot;</span>&lt;&lt;endl;</div><div class="line"><a name="l00361"></a><span class="lineno">  361</span>&#160;                    cout&lt;&lt;Twb.translation()&lt;&lt;std::endl;</div><div class="line"><a name="l00362"></a><span class="lineno">  362</span>&#160;                    <span class="comment">// in their local body axis </span></div><div class="line"><a name="l00363"></a><span class="lineno">  363</span>&#160;                    lower_limit(0) = -l/2 - safe_r;</div><div class="line"><a name="l00364"></a><span class="lineno">  364</span>&#160;                    lower_limit(1) = -safe_r;</div><div class="line"><a name="l00365"></a><span class="lineno">  365</span>&#160;                    lower_limit(2) = -safe_r;</div><div class="line"><a name="l00366"></a><span class="lineno">  366</span>&#160;</div><div class="line"><a name="l00367"></a><span class="lineno">  367</span>&#160;                    upper_limit = -lower_limit;</div><div class="line"><a name="l00368"></a><span class="lineno">  368</span>&#160;</div><div class="line"><a name="l00369"></a><span class="lineno">  369</span>&#160;                    <span class="comment">// in world frame </span></div><div class="line"><a name="l00370"></a><span class="lineno">  370</span>&#160;                    lower_limit = Twb*lower_limit;</div><div class="line"><a name="l00371"></a><span class="lineno">  371</span>&#160;                    upper_limit = Twb*upper_limit;</div><div class="line"><a name="l00372"></a><span class="lineno">  372</span>&#160;</div><div class="line"><a name="l00373"></a><span class="lineno">  373</span>&#160;                    <span class="comment">// marker </span></div><div class="line"><a name="l00374"></a><span class="lineno">  374</span>&#160;                    </div><div class="line"><a name="l00375"></a><span class="lineno">  375</span>&#160;                    <span class="comment">// pose </span></div><div class="line"><a name="l00376"></a><span class="lineno">  376</span>&#160;                    safe_corridor_marker_single_base.pose.position.x = Twb.translation()(0);                     </div><div class="line"><a name="l00377"></a><span class="lineno">  377</span>&#160;                    safe_corridor_marker_single_base.pose.position.y = Twb.translation()(1);                     </div><div class="line"><a name="l00378"></a><span class="lineno">  378</span>&#160;                    safe_corridor_marker_single_base.pose.position.z = Twb.translation()(2);                     </div><div class="line"><a name="l00379"></a><span class="lineno">  379</span>&#160;</div><div class="line"><a name="l00380"></a><span class="lineno">  380</span>&#160;                    Quaternionf q(Twb.rotation());</div><div class="line"><a name="l00381"></a><span class="lineno">  381</span>&#160;                    safe_corridor_marker_single_base.pose.orientation.x = q.x();</div><div class="line"><a name="l00382"></a><span class="lineno">  382</span>&#160;                    safe_corridor_marker_single_base.pose.orientation.y = q.y();</div><div class="line"><a name="l00383"></a><span class="lineno">  383</span>&#160;                    safe_corridor_marker_single_base.pose.orientation.z = q.z();</div><div class="line"><a name="l00384"></a><span class="lineno">  384</span>&#160;                    safe_corridor_marker_single_base.pose.orientation.w = q.w();</div><div class="line"><a name="l00385"></a><span class="lineno">  385</span>&#160;                    </div><div class="line"><a name="l00386"></a><span class="lineno">  386</span>&#160;                    <span class="comment">// scale </span></div><div class="line"><a name="l00387"></a><span class="lineno">  387</span>&#160;                    safe_corridor_marker_single_base.scale.x = 2*(l/2 + safe_r);</div><div class="line"><a name="l00388"></a><span class="lineno">  388</span>&#160;                    safe_corridor_marker_single_base.scale.y = 2*(safe_r);</div><div class="line"><a name="l00389"></a><span class="lineno">  389</span>&#160;                    safe_corridor_marker_single_base.scale.z = 2*(safe_r);</div><div class="line"><a name="l00390"></a><span class="lineno">  390</span>&#160;                    safe_corridor_marker_single_base.id = n;</div><div class="line"><a name="l00391"></a><span class="lineno">  391</span>&#160;                    <span class="comment">// append </span></div><div class="line"><a name="l00392"></a><span class="lineno">  392</span>&#160;                    safe_corridor_marker_single_array.markers.push_back(safe_corridor_marker_single_base);</div><div class="line"><a name="l00393"></a><span class="lineno">  393</span>&#160;                }</div><div class="line"><a name="l00394"></a><span class="lineno">  394</span>&#160;</div><div class="line"><a name="l00395"></a><span class="lineno">  395</span>&#160;                A_sub.block(ineq_row_insert_idx,ineq_col_insert_idx1,1,blck_size)=-<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,t_control,0).transpose();        </div><div class="line"><a name="l00396"></a><span class="lineno">  396</span>&#160;                            </div><div class="line"><a name="l00397"></a><span class="lineno">  397</span>&#160;                <span class="comment">// lower limit </span></div><div class="line"><a name="l00398"></a><span class="lineno">  398</span>&#160;                bx_sub.coeffRef(ineq_row_insert_idx) = -lower_limit(0);</div><div class="line"><a name="l00399"></a><span class="lineno">  399</span>&#160;                by_sub.coeffRef(ineq_row_insert_idx) = -lower_limit(1);</div><div class="line"><a name="l00400"></a><span class="lineno">  400</span>&#160;                bz_sub.coeffRef(ineq_row_insert_idx) = -lower_limit(2);        </div><div class="line"><a name="l00401"></a><span class="lineno">  401</span>&#160;</div><div class="line"><a name="l00402"></a><span class="lineno">  402</span>&#160;                ineq_row_insert_idx++;</div><div class="line"><a name="l00403"></a><span class="lineno">  403</span>&#160;</div><div class="line"><a name="l00404"></a><span class="lineno">  404</span>&#160;                <span class="comment">// upper limit </span></div><div class="line"><a name="l00405"></a><span class="lineno">  405</span>&#160;                A_sub.block(ineq_row_insert_idx,ineq_col_insert_idx1,1,blck_size)=<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,t_control,0).transpose();        </div><div class="line"><a name="l00406"></a><span class="lineno">  406</span>&#160;                bx_sub.coeffRef(ineq_row_insert_idx) = upper_limit(0);</div><div class="line"><a name="l00407"></a><span class="lineno">  407</span>&#160;                by_sub.coeffRef(ineq_row_insert_idx) = upper_limit(1);</div><div class="line"><a name="l00408"></a><span class="lineno">  408</span>&#160;                bz_sub.coeffRef(ineq_row_insert_idx) = upper_limit(2);      </div><div class="line"><a name="l00409"></a><span class="lineno">  409</span>&#160;</div><div class="line"><a name="l00410"></a><span class="lineno">  410</span>&#160;                ineq_row_insert_idx++;</div><div class="line"><a name="l00411"></a><span class="lineno">  411</span>&#160;</div><div class="line"><a name="l00412"></a><span class="lineno">  412</span>&#160;            }</div><div class="line"><a name="l00413"></a><span class="lineno">  413</span>&#160;            ineq_col_insert_idx1 += blck_size;</div><div class="line"><a name="l00414"></a><span class="lineno">  414</span>&#160;        }</div><div class="line"><a name="l00415"></a><span class="lineno">  415</span>&#160;</div><div class="line"><a name="l00416"></a><span class="lineno">  416</span>&#160;        <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(Aineq_x,A_sub); <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(bineq_x,bx_sub);</div><div class="line"><a name="l00417"></a><span class="lineno">  417</span>&#160;        <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(Aineq_y,A_sub); <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(bineq_y,by_sub);</div><div class="line"><a name="l00418"></a><span class="lineno">  418</span>&#160;        <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(Aineq_z,A_sub); <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(bineq_z,bz_sub); </div><div class="line"><a name="l00419"></a><span class="lineno">  419</span>&#160;</div><div class="line"><a name="l00420"></a><span class="lineno">  420</span>&#160;    </div><div class="line"><a name="l00421"></a><span class="lineno">  421</span>&#160;    }<span class="keywordflow">else</span>{</div><div class="line"><a name="l00422"></a><span class="lineno">  422</span>&#160;        safe_corridor_marker.points.clear();</div><div class="line"><a name="l00423"></a><span class="lineno">  423</span>&#160;        safe_corridor_marker_single_array.markers.clear();</div><div class="line"><a name="l00424"></a><span class="lineno">  424</span>&#160;    }</div><div class="line"><a name="l00425"></a><span class="lineno">  425</span>&#160;</div><div class="line"><a name="l00426"></a><span class="lineno">  426</span>&#160;    <span class="comment">/*</span></div><div class="line"><a name="l00427"></a><span class="lineno">  427</span>&#160;<span class="comment">        4. Wrapping  </span></div><div class="line"><a name="l00428"></a><span class="lineno">  428</span>&#160;<span class="comment">    */</span></div><div class="line"><a name="l00429"></a><span class="lineno">  429</span>&#160;    <span class="comment">//  -----------------------------------------------------------------------    </span></div><div class="line"><a name="l00430"></a><span class="lineno">  430</span>&#160;</div><div class="line"><a name="l00431"></a><span class="lineno">  431</span>&#160;    <a class="code" href="struct_q_p__form.html">QP_form</a> qp_x; qp_x.<a class="code" href="struct_q_p__form.html#a5613317ae662dd137324ee52ca52b1ae">Q</a> = Qx; qp_x.<a class="code" href="struct_q_p__form.html#ac8ff933f268484b2f78b77ffc056f11d">H</a> = Hx; qp_x.<a class="code" href="struct_q_p__form.html#afeb66f78f73f929e99f6f8afcfcc17b1">A</a> = Aineq_x; qp_x.<a class="code" href="struct_q_p__form.html#a949cd1a76929ca7aeb61c8955da4a1a2">b</a> = bineq_x; qp_x.<a class="code" href="struct_q_p__form.html#af0fab4bf47d9b4a2598445bd11f446e0">Aeq</a> = Aeq_x; qp_x.<a class="code" href="struct_q_p__form.html#a3671f01cebe0e9acdb6ce105b6eff780">beq</a> = beq_x; </div><div class="line"><a name="l00432"></a><span class="lineno">  432</span>&#160;    <a class="code" href="struct_q_p__form.html">QP_form</a> qp_y; qp_y.<a class="code" href="struct_q_p__form.html#a5613317ae662dd137324ee52ca52b1ae">Q</a> = Qy; qp_y.<a class="code" href="struct_q_p__form.html#ac8ff933f268484b2f78b77ffc056f11d">H</a> = Hy; qp_y.<a class="code" href="struct_q_p__form.html#afeb66f78f73f929e99f6f8afcfcc17b1">A</a> = Aineq_y; qp_y.<a class="code" href="struct_q_p__form.html#a949cd1a76929ca7aeb61c8955da4a1a2">b</a> = bineq_y; qp_y.<a class="code" href="struct_q_p__form.html#af0fab4bf47d9b4a2598445bd11f446e0">Aeq</a> = Aeq_y; qp_y.<a class="code" href="struct_q_p__form.html#a3671f01cebe0e9acdb6ce105b6eff780">beq</a> = beq_y; </div><div class="line"><a name="l00433"></a><span class="lineno">  433</span>&#160;    <a class="code" href="struct_q_p__form.html">QP_form</a> qp_z; qp_z.<a class="code" href="struct_q_p__form.html#a5613317ae662dd137324ee52ca52b1ae">Q</a> = Qz; qp_z.<a class="code" href="struct_q_p__form.html#ac8ff933f268484b2f78b77ffc056f11d">H</a> = Hz; qp_z.<a class="code" href="struct_q_p__form.html#afeb66f78f73f929e99f6f8afcfcc17b1">A</a> = Aineq_z; qp_z.<a class="code" href="struct_q_p__form.html#a949cd1a76929ca7aeb61c8955da4a1a2">b</a> = bineq_z; qp_z.<a class="code" href="struct_q_p__form.html#af0fab4bf47d9b4a2598445bd11f446e0">Aeq</a> = Aeq_z; qp_z.<a class="code" href="struct_q_p__form.html#a3671f01cebe0e9acdb6ce105b6eff780">beq</a> = beq_z; </div><div class="line"><a name="l00434"></a><span class="lineno">  434</span>&#160;    </div><div class="line"><a name="l00435"></a><span class="lineno">  435</span>&#160;    <a class="code" href="struct_q_p__form__xyz.html">QP_form_xyz</a> qp_prob_xyz;</div><div class="line"><a name="l00436"></a><span class="lineno">  436</span>&#160;    qp_prob_xyz.<a class="code" href="struct_q_p__form__xyz.html#a3a6e7a104ef31fda66007013ba3741f8">x</a> = qp_x;</div><div class="line"><a name="l00437"></a><span class="lineno">  437</span>&#160;    qp_prob_xyz.<a class="code" href="struct_q_p__form__xyz.html#a3e1c8fbfd5a4650fb45fc0ca14c2bcb6">y</a> = qp_y;</div><div class="line"><a name="l00438"></a><span class="lineno">  438</span>&#160;    qp_prob_xyz.<a class="code" href="struct_q_p__form__xyz.html#a243478dd3ac49ecf0d6fceffbcdf408b">z</a> = qp_z;</div><div class="line"><a name="l00439"></a><span class="lineno">  439</span>&#160;    </div><div class="line"><a name="l00440"></a><span class="lineno">  440</span>&#160;    <span class="keywordflow">return</span> qp_prob_xyz;</div><div class="line"><a name="l00441"></a><span class="lineno">  441</span>&#160;};</div><div class="line"><a name="l00442"></a><span class="lineno">  442</span>&#160;</div><div class="line"><a name="l00443"></a><span class="lineno">  443</span>&#160;</div><div class="line"><a name="l00444"></a><span class="lineno">  444</span>&#160;<span class="comment">/*</span></div><div class="line"><a name="l00445"></a><span class="lineno">  445</span>&#160;<span class="comment">    sub routines   </span></div><div class="line"><a name="l00446"></a><span class="lineno">  446</span>&#160;<span class="comment">*/</span></div><div class="line"><a name="l00447"></a><span class="lineno">  447</span>&#160;</div><div class="line"><a name="l00448"></a><span class="lineno">  448</span>&#160;<span class="comment">//  -----------------------------------------------------------------------    </span></div><div class="line"><a name="l00449"></a><span class="lineno">  449</span>&#160;</div><div class="line"><a name="l00450"></a><span class="lineno"><a class="line" href="class_path_planner.html#ad231acc2bce7d5e143fa49c69515c4a9">  450</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="class_path_planner.html#ad231acc2bce7d5e143fa49c69515c4a9">PathPlanner::horizon_eval_spline</a>(<span class="keywordtype">int</span> N_eval_interval){</div><div class="line"><a name="l00451"></a><span class="lineno">  451</span>&#160;</div><div class="line"><a name="l00452"></a><span class="lineno">  452</span>&#160;    geometry_msgs::PoseStamped poseStamped;</div><div class="line"><a name="l00453"></a><span class="lineno">  453</span>&#160;    nav_msgs::Path path; </div><div class="line"><a name="l00454"></a><span class="lineno">  454</span>&#160;</div><div class="line"><a name="l00455"></a><span class="lineno">  455</span>&#160;    ROS_INFO(<span class="stringliteral">&quot;evaluating spline....&quot;</span>);</div><div class="line"><a name="l00456"></a><span class="lineno">  456</span>&#160;</div><div class="line"><a name="l00457"></a><span class="lineno">  457</span>&#160;    <span class="keywordtype">int</span> n_seg=spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a1315136ffc90e4164123f07aef230a6e">n_seg</a>;</div><div class="line"><a name="l00458"></a><span class="lineno">  458</span>&#160;    <span class="keywordtype">int</span> poly_order = spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a868db24a21948436dd8cbe7c71241bc1">poly_order</a>;</div><div class="line"><a name="l00459"></a><span class="lineno">  459</span>&#160;    </div><div class="line"><a name="l00460"></a><span class="lineno">  460</span>&#160;    printf(<span class="stringliteral">&quot;n_seg : %d, poly_order: %d, \n&quot;</span>,spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a1315136ffc90e4164123f07aef230a6e">n_seg</a>,spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a868db24a21948436dd8cbe7c71241bc1">poly_order</a>);</div><div class="line"><a name="l00461"></a><span class="lineno">  461</span>&#160;</div><div class="line"><a name="l00462"></a><span class="lineno">  462</span>&#160;    <span class="comment">// std::cout&lt;&lt;spline.knot_time.size()&lt;&lt;std::endl;</span></div><div class="line"><a name="l00463"></a><span class="lineno">  463</span>&#160;    <span class="comment">// per each segment</span></div><div class="line"><a name="l00464"></a><span class="lineno">  464</span>&#160;    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> n=0;n&lt;n_seg;n++){</div><div class="line"><a name="l00465"></a><span class="lineno">  465</span>&#160;        <span class="comment">// evaluation start and end</span></div><div class="line"><a name="l00466"></a><span class="lineno">  466</span>&#160;        </div><div class="line"><a name="l00467"></a><span class="lineno">  467</span>&#160;        <span class="keywordtype">double</span> t1=spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>[n];</div><div class="line"><a name="l00468"></a><span class="lineno">  468</span>&#160;        <span class="keywordtype">double</span> t2=spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>[n+1];</div><div class="line"><a name="l00469"></a><span class="lineno">  469</span>&#160;</div><div class="line"><a name="l00470"></a><span class="lineno">  470</span>&#160;        <span class="comment">// time horizon between start and end</span></div><div class="line"><a name="l00471"></a><span class="lineno">  471</span>&#160;        VectorXd eval_time_horizon(N_eval_interval);</div><div class="line"><a name="l00472"></a><span class="lineno">  472</span>&#160;        eval_time_horizon.setLinSpaced(N_eval_interval,t1,t2);</div><div class="line"><a name="l00473"></a><span class="lineno">  473</span>&#160;</div><div class="line"><a name="l00474"></a><span class="lineno">  474</span>&#160;        <span class="comment">// evaluation path on that horizon</span></div><div class="line"><a name="l00475"></a><span class="lineno">  475</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> t_idx=0;t_idx&lt;N_eval_interval;t_idx++){</div><div class="line"><a name="l00476"></a><span class="lineno">  476</span>&#160;            <span class="comment">// double t_eval=(eval_time_horizon.coeff(t_idx)-t1)/(t2-t1);</span></div><div class="line"><a name="l00477"></a><span class="lineno">  477</span>&#160;            <span class="keywordtype">double</span> t_eval=(eval_time_horizon.coeff(t_idx)-t1);</div><div class="line"><a name="l00478"></a><span class="lineno">  478</span>&#160;</div><div class="line"><a name="l00479"></a><span class="lineno">  479</span>&#160;            poseStamped.pose.position.x=<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,t_eval,0).transpose()*Map&lt;VectorXd&gt;(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a0e2ce289d7bb5fbcfe7e436c3c24b3a6">spline_x</a>.<a class="code" href="structtraj__gen_1_1_poly_spline__.html#a0f2e8bfe261fae6f2b762c093c0d692d">poly_coeff</a>[n].coeff.data(),poly_order+1);</div><div class="line"><a name="l00480"></a><span class="lineno">  480</span>&#160;            poseStamped.pose.position.y=<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,t_eval,0).transpose()*Map&lt;VectorXd&gt;(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a149445d98f325cd01096a829e4ac90d8">spline_y</a>.<a class="code" href="structtraj__gen_1_1_poly_spline__.html#a0f2e8bfe261fae6f2b762c093c0d692d">poly_coeff</a>[n].coeff.data(),poly_order+1);</div><div class="line"><a name="l00481"></a><span class="lineno">  481</span>&#160;            poseStamped.pose.position.z=<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,t_eval,0).transpose()*Map&lt;VectorXd&gt;(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a60e87b9c4f18726568ef29bb43fba580">spline_z</a>.<a class="code" href="structtraj__gen_1_1_poly_spline__.html#a0f2e8bfe261fae6f2b762c093c0d692d">poly_coeff</a>[n].coeff.data(),poly_order+1);</div><div class="line"><a name="l00482"></a><span class="lineno">  482</span>&#160;            path.poses.push_back(poseStamped);</div><div class="line"><a name="l00483"></a><span class="lineno">  483</span>&#160;        }</div><div class="line"><a name="l00484"></a><span class="lineno">  484</span>&#160;    }</div><div class="line"><a name="l00485"></a><span class="lineno">  485</span>&#160;</div><div class="line"><a name="l00486"></a><span class="lineno">  486</span>&#160;     current_path=path;</div><div class="line"><a name="l00487"></a><span class="lineno">  487</span>&#160;</div><div class="line"><a name="l00488"></a><span class="lineno">  488</span>&#160;}</div><div class="line"><a name="l00489"></a><span class="lineno">  489</span>&#160;</div><div class="line"><a name="l00490"></a><span class="lineno">  490</span>&#160;</div><div class="line"><a name="l00491"></a><span class="lineno"><a class="line" href="class_path_planner.html#aa12888c52d7cb2d429c3782060b9b45e">  491</a></span>&#160;geometry_msgs::Point <a class="code" href="class_path_planner.html#aa12888c52d7cb2d429c3782060b9b45e">PathPlanner::point_eval_spline</a>(<span class="keywordtype">double</span> t_eval) {</div><div class="line"><a name="l00492"></a><span class="lineno">  492</span>&#160;</div><div class="line"><a name="l00493"></a><span class="lineno">  493</span>&#160;</div><div class="line"><a name="l00494"></a><span class="lineno">  494</span>&#160;    geometry_msgs::Point eval_point;</div><div class="line"><a name="l00495"></a><span class="lineno">  495</span>&#160;</div><div class="line"><a name="l00496"></a><span class="lineno">  496</span>&#160;    <span class="keywordtype">int</span> poly_order=spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a868db24a21948436dd8cbe7c71241bc1">poly_order</a>;</div><div class="line"><a name="l00497"></a><span class="lineno">  497</span>&#160;    <span class="comment">// DEBUG</span></div><div class="line"><a name="l00498"></a><span class="lineno">  498</span>&#160;<span class="comment">//          std::cout&lt;&lt;&quot;knot time of this: &quot;&lt;&lt;std::endl;</span></div><div class="line"><a name="l00499"></a><span class="lineno">  499</span>&#160;    <span class="keywordflow">for</span>(<span class="keyword">auto</span> it = spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>.begin();it&lt;spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>.end();it++)</div><div class="line"><a name="l00500"></a><span class="lineno">  500</span>&#160;<span class="comment">//      std::cout&lt;&lt;*it&lt;&lt;&quot;, &quot;;spline_xyz</span></div><div class="line"><a name="l00501"></a><span class="lineno">  501</span>&#160;<span class="comment">//  std::cout&lt;&lt;std::endl;</span></div><div class="line"><a name="l00502"></a><span class="lineno">  502</span>&#160;<span class="comment">//    std::cout&lt;&lt;&quot;point_eval: &quot;&lt;&lt;t_eval.toSec()&lt;&lt;&quot;knot time final: &quot;&lt;&lt;spline.knot_time.back()&lt;&lt;std::endl;</span></div><div class="line"><a name="l00503"></a><span class="lineno">  503</span>&#160;    t_eval =min(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>.back(),t_eval);</div><div class="line"><a name="l00504"></a><span class="lineno">  504</span>&#160;    t_eval =max(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>.front(),t_eval);</div><div class="line"><a name="l00505"></a><span class="lineno">  505</span>&#160;</div><div class="line"><a name="l00506"></a><span class="lineno">  506</span>&#160;    Eigen::Index spline_idx=<a class="code" href="_poly_traj_gen_8cpp.html#a17e17da0f42201d595257679c8ae6170">find_spline_interval</a>(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>,t_eval);</div><div class="line"><a name="l00507"></a><span class="lineno">  507</span>&#160;<span class="comment">//  std::cout&lt;&lt;&quot;Index: &quot;&lt;&lt;spline_idx&lt;&lt;std::endl;</span></div><div class="line"><a name="l00508"></a><span class="lineno">  508</span>&#160;    <span class="comment">// double t_eval_norm = (t_eval-spline_xyz.knot_time[spline_idx])/(spline_xyz.knot_time[spline_idx+1]-spline_xyz.knot_time[spline_idx]);</span></div><div class="line"><a name="l00509"></a><span class="lineno">  509</span>&#160;    <span class="keywordtype">double</span> t_eval_norm = (t_eval-spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>[spline_idx]);</div><div class="line"><a name="l00510"></a><span class="lineno">  510</span>&#160;</div><div class="line"><a name="l00511"></a><span class="lineno">  511</span>&#160;    eval_point.x=<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,t_eval_norm,0).transpose()*Map&lt;VectorXd&gt;(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a0e2ce289d7bb5fbcfe7e436c3c24b3a6">spline_x</a>.<a class="code" href="structtraj__gen_1_1_poly_spline__.html#a0f2e8bfe261fae6f2b762c093c0d692d">poly_coeff</a>[spline_idx].coeff.data(),poly_order+1);</div><div class="line"><a name="l00512"></a><span class="lineno">  512</span>&#160;    eval_point.y=<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,t_eval_norm,0).transpose()*Map&lt;VectorXd&gt;(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a149445d98f325cd01096a829e4ac90d8">spline_y</a>.<a class="code" href="structtraj__gen_1_1_poly_spline__.html#a0f2e8bfe261fae6f2b762c093c0d692d">poly_coeff</a>[spline_idx].coeff.data(),poly_order+1);</div><div class="line"><a name="l00513"></a><span class="lineno">  513</span>&#160;    eval_point.z=<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,t_eval_norm,0).transpose()*Map&lt;VectorXd&gt;(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a60e87b9c4f18726568ef29bb43fba580">spline_z</a>.<a class="code" href="structtraj__gen_1_1_poly_spline__.html#a0f2e8bfe261fae6f2b762c093c0d692d">poly_coeff</a>[spline_idx].coeff.data(),poly_order+1);</div><div class="line"><a name="l00514"></a><span class="lineno">  514</span>&#160;    </div><div class="line"><a name="l00515"></a><span class="lineno">  515</span>&#160;    <span class="keywordflow">return</span> eval_point;</div><div class="line"><a name="l00516"></a><span class="lineno">  516</span>&#160;}</div><div class="line"><a name="l00517"></a><span class="lineno">  517</span>&#160;</div><div class="line"><a name="l00518"></a><span class="lineno"><a class="line" href="class_path_planner.html#ae31bac84392ea0fbe59edfc7582215e0">  518</a></span>&#160;geometry_msgs::Twist <a class="code" href="class_path_planner.html#ae31bac84392ea0fbe59edfc7582215e0">PathPlanner::vel_eval_spline</a>(<span class="keywordtype">double</span> t_eval){</div><div class="line"><a name="l00519"></a><span class="lineno">  519</span>&#160;</div><div class="line"><a name="l00520"></a><span class="lineno">  520</span>&#160;    geometry_msgs::Twist eval_vel;</div><div class="line"><a name="l00521"></a><span class="lineno">  521</span>&#160;    <span class="keywordtype">int</span> poly_order=spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a868db24a21948436dd8cbe7c71241bc1">poly_order</a>;</div><div class="line"><a name="l00522"></a><span class="lineno">  522</span>&#160;    t_eval =min(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>.back(),t_eval);</div><div class="line"><a name="l00523"></a><span class="lineno">  523</span>&#160;    Eigen::Index spline_idx=<a class="code" href="_poly_traj_gen_8cpp.html#a17e17da0f42201d595257679c8ae6170">find_spline_interval</a>(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>,t_eval);  </div><div class="line"><a name="l00524"></a><span class="lineno">  524</span>&#160;    <span class="keywordtype">double</span> t_eval_norm = (t_eval-spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>[spline_idx]);</div><div class="line"><a name="l00525"></a><span class="lineno">  525</span>&#160;</div><div class="line"><a name="l00526"></a><span class="lineno">  526</span>&#160;    eval_vel.linear.x=<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,t_eval_norm,1).transpose()*Map&lt;VectorXd&gt;(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a0e2ce289d7bb5fbcfe7e436c3c24b3a6">spline_x</a>.<a class="code" href="structtraj__gen_1_1_poly_spline__.html#a0f2e8bfe261fae6f2b762c093c0d692d">poly_coeff</a>[spline_idx].coeff.data(),poly_order+1);</div><div class="line"><a name="l00527"></a><span class="lineno">  527</span>&#160;    eval_vel.linear.y=<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,t_eval_norm,1).transpose()*Map&lt;VectorXd&gt;(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a149445d98f325cd01096a829e4ac90d8">spline_y</a>.<a class="code" href="structtraj__gen_1_1_poly_spline__.html#a0f2e8bfe261fae6f2b762c093c0d692d">poly_coeff</a>[spline_idx].coeff.data(),poly_order+1);</div><div class="line"><a name="l00528"></a><span class="lineno">  528</span>&#160;    eval_vel.linear.z=<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,t_eval_norm,1).transpose()*Map&lt;VectorXd&gt;(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a60e87b9c4f18726568ef29bb43fba580">spline_z</a>.<a class="code" href="structtraj__gen_1_1_poly_spline__.html#a0f2e8bfe261fae6f2b762c093c0d692d">poly_coeff</a>[spline_idx].coeff.data(),poly_order+1);</div><div class="line"><a name="l00529"></a><span class="lineno">  529</span>&#160;    </div><div class="line"><a name="l00530"></a><span class="lineno">  530</span>&#160;    <span class="keywordflow">return</span> eval_vel;</div><div class="line"><a name="l00531"></a><span class="lineno">  531</span>&#160;</div><div class="line"><a name="l00532"></a><span class="lineno">  532</span>&#160;}</div><div class="line"><a name="l00533"></a><span class="lineno">  533</span>&#160;</div><div class="line"><a name="l00534"></a><span class="lineno"><a class="line" href="class_path_planner.html#ad1ad0eb3d19d6f1e9ab921e835361c14">  534</a></span>&#160;geometry_msgs::Twist <a class="code" href="class_path_planner.html#ad1ad0eb3d19d6f1e9ab921e835361c14">PathPlanner::accel_eval_spline</a>(<span class="keywordtype">double</span> t_eval){</div><div class="line"><a name="l00535"></a><span class="lineno">  535</span>&#160;</div><div class="line"><a name="l00536"></a><span class="lineno">  536</span>&#160;    geometry_msgs::Twist eval_acc;</div><div class="line"><a name="l00537"></a><span class="lineno">  537</span>&#160;    <span class="keywordtype">int</span> poly_order=spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a868db24a21948436dd8cbe7c71241bc1">poly_order</a>;</div><div class="line"><a name="l00538"></a><span class="lineno">  538</span>&#160;    t_eval =min(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>.back(),t_eval);</div><div class="line"><a name="l00539"></a><span class="lineno">  539</span>&#160;    Eigen::Index spline_idx=<a class="code" href="_poly_traj_gen_8cpp.html#a17e17da0f42201d595257679c8ae6170">find_spline_interval</a>(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>,t_eval);  </div><div class="line"><a name="l00540"></a><span class="lineno">  540</span>&#160;    <span class="keywordtype">double</span> t_eval_norm = (t_eval-spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>[spline_idx]);</div><div class="line"><a name="l00541"></a><span class="lineno">  541</span>&#160;</div><div class="line"><a name="l00542"></a><span class="lineno">  542</span>&#160;    eval_acc.linear.x=<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,t_eval_norm,2).transpose()*Map&lt;VectorXd&gt;(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a0e2ce289d7bb5fbcfe7e436c3c24b3a6">spline_x</a>.<a class="code" href="structtraj__gen_1_1_poly_spline__.html#a0f2e8bfe261fae6f2b762c093c0d692d">poly_coeff</a>[spline_idx].coeff.data(),poly_order+1);</div><div class="line"><a name="l00543"></a><span class="lineno">  543</span>&#160;    eval_acc.linear.y=<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,t_eval_norm,2).transpose()*Map&lt;VectorXd&gt;(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a149445d98f325cd01096a829e4ac90d8">spline_y</a>.<a class="code" href="structtraj__gen_1_1_poly_spline__.html#a0f2e8bfe261fae6f2b762c093c0d692d">poly_coeff</a>[spline_idx].coeff.data(),poly_order+1);</div><div class="line"><a name="l00544"></a><span class="lineno">  544</span>&#160;    eval_acc.linear.z=<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,t_eval_norm,2).transpose()*Map&lt;VectorXd&gt;(spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a60e87b9c4f18726568ef29bb43fba580">spline_z</a>.<a class="code" href="structtraj__gen_1_1_poly_spline__.html#a0f2e8bfe261fae6f2b762c093c0d692d">poly_coeff</a>[spline_idx].coeff.data(),poly_order+1);</div><div class="line"><a name="l00545"></a><span class="lineno">  545</span>&#160;    </div><div class="line"><a name="l00546"></a><span class="lineno">  546</span>&#160;    <span class="keywordflow">return</span> eval_acc;</div><div class="line"><a name="l00547"></a><span class="lineno">  547</span>&#160;</div><div class="line"><a name="l00548"></a><span class="lineno">  548</span>&#160;}</div><div class="line"><a name="l00549"></a><span class="lineno">  549</span>&#160;</div><div class="line"><a name="l00550"></a><span class="lineno">  550</span>&#160;</div><div class="line"><a name="l00551"></a><span class="lineno">  551</span>&#160;</div><div class="line"><a name="l00552"></a><span class="lineno"><a class="line" href="class_path_planner.html#a6fd581289799d1daac012050fb712a0c">  552</a></span>&#160;VectorXd <a class="code" href="class_path_planner.html#a6fd581289799d1daac012050fb712a0c">PathPlanner::solveqp</a>(<a class="code" href="struct_q_p__form.html">QP_form</a> qp_prob,<span class="keywordtype">bool</span>&amp; is_ok){</div><div class="line"><a name="l00553"></a><span class="lineno">  553</span>&#160;    is_ok = <span class="keyword">true</span>;</div><div class="line"><a name="l00554"></a><span class="lineno">  554</span>&#160;    MatrixXd Q = qp_prob.<a class="code" href="struct_q_p__form.html#a5613317ae662dd137324ee52ca52b1ae">Q</a>;</div><div class="line"><a name="l00555"></a><span class="lineno">  555</span>&#160;    MatrixXd H = qp_prob.<a class="code" href="struct_q_p__form.html#ac8ff933f268484b2f78b77ffc056f11d">H</a>;</div><div class="line"><a name="l00556"></a><span class="lineno">  556</span>&#160;    MatrixXd Aineq = qp_prob.<a class="code" href="struct_q_p__form.html#afeb66f78f73f929e99f6f8afcfcc17b1">A</a>;</div><div class="line"><a name="l00557"></a><span class="lineno">  557</span>&#160;    MatrixXd bineq = qp_prob.<a class="code" href="struct_q_p__form.html#a949cd1a76929ca7aeb61c8955da4a1a2">b</a>;</div><div class="line"><a name="l00558"></a><span class="lineno">  558</span>&#160;    MatrixXd Aeq = qp_prob.<a class="code" href="struct_q_p__form.html#af0fab4bf47d9b4a2598445bd11f446e0">Aeq</a>;</div><div class="line"><a name="l00559"></a><span class="lineno">  559</span>&#160;    MatrixXd beq = qp_prob.<a class="code" href="struct_q_p__form.html#a3671f01cebe0e9acdb6ce105b6eff780">beq</a>;</div><div class="line"><a name="l00560"></a><span class="lineno">  560</span>&#160;    <span class="keywordflow">if</span>(is_this_verbose){</div><div class="line"><a name="l00561"></a><span class="lineno">  561</span>&#160;        cout&lt;&lt;<span class="stringliteral">&quot;Q: &quot;</span>&lt;&lt;endl;</div><div class="line"><a name="l00562"></a><span class="lineno">  562</span>&#160;        cout&lt;&lt;Q&lt;&lt;endl;</div><div class="line"><a name="l00563"></a><span class="lineno">  563</span>&#160;        </div><div class="line"><a name="l00564"></a><span class="lineno">  564</span>&#160;        cout&lt;&lt;<span class="stringliteral">&quot;H: &quot;</span>&lt;&lt;endl;</div><div class="line"><a name="l00565"></a><span class="lineno">  565</span>&#160;        cout&lt;&lt;H&lt;&lt;endl;</div><div class="line"><a name="l00566"></a><span class="lineno">  566</span>&#160;            </div><div class="line"><a name="l00567"></a><span class="lineno">  567</span>&#160;        cout&lt;&lt;<span class="stringliteral">&quot;Aineq: &quot;</span>&lt;&lt;endl;</div><div class="line"><a name="l00568"></a><span class="lineno">  568</span>&#160;        cout&lt;&lt;Aineq&lt;&lt;endl;        </div><div class="line"><a name="l00569"></a><span class="lineno">  569</span>&#160;</div><div class="line"><a name="l00570"></a><span class="lineno">  570</span>&#160;        cout&lt;&lt;<span class="stringliteral">&quot;bineq: &quot;</span>&lt;&lt;endl;</div><div class="line"><a name="l00571"></a><span class="lineno">  571</span>&#160;        cout&lt;&lt;bineq&lt;&lt;endl;  </div><div class="line"><a name="l00572"></a><span class="lineno">  572</span>&#160;</div><div class="line"><a name="l00573"></a><span class="lineno">  573</span>&#160;        cout&lt;&lt;<span class="stringliteral">&quot;Aeq: &quot;</span>&lt;&lt;endl;</div><div class="line"><a name="l00574"></a><span class="lineno">  574</span>&#160;        cout&lt;&lt;Aeq&lt;&lt;endl;        </div><div class="line"><a name="l00575"></a><span class="lineno">  575</span>&#160;</div><div class="line"><a name="l00576"></a><span class="lineno">  576</span>&#160;        cout&lt;&lt;<span class="stringliteral">&quot;beq: &quot;</span>&lt;&lt;endl;</div><div class="line"><a name="l00577"></a><span class="lineno">  577</span>&#160;        cout&lt;&lt;beq&lt;&lt;endl;  </div><div class="line"><a name="l00578"></a><span class="lineno">  578</span>&#160;    }</div><div class="line"><a name="l00579"></a><span class="lineno">  579</span>&#160;    USING_NAMESPACE_QPOASES;</div><div class="line"><a name="l00580"></a><span class="lineno">  580</span>&#160;</div><div class="line"><a name="l00581"></a><span class="lineno">  581</span>&#160;    <span class="keywordtype">int</span> N_var = Q.rows();</div><div class="line"><a name="l00582"></a><span class="lineno">  582</span>&#160;    <span class="keywordtype">int</span> N_ineq_const = Aineq.rows();</div><div class="line"><a name="l00583"></a><span class="lineno">  583</span>&#160;    <span class="keywordtype">int</span> N_eq_const = Aeq.rows();    </div><div class="line"><a name="l00584"></a><span class="lineno">  584</span>&#160;    <span class="keywordtype">int</span> N_const = N_ineq_const + N_eq_const;</div><div class="line"><a name="l00585"></a><span class="lineno">  585</span>&#160;</div><div class="line"><a name="l00586"></a><span class="lineno">  586</span>&#160;    real_t H_qp[N_var*N_var];</div><div class="line"><a name="l00587"></a><span class="lineno">  587</span>&#160;    real_t g[N_var];</div><div class="line"><a name="l00588"></a><span class="lineno">  588</span>&#160;    real_t A[N_var*N_const];</div><div class="line"><a name="l00589"></a><span class="lineno">  589</span>&#160;        real_t lbA[N_const];</div><div class="line"><a name="l00590"></a><span class="lineno">  590</span>&#160;    real_t ubA[N_const];</div><div class="line"><a name="l00591"></a><span class="lineno">  591</span>&#160;</div><div class="line"><a name="l00592"></a><span class="lineno">  592</span>&#160;    <span class="comment">// cost</span></div><div class="line"><a name="l00593"></a><span class="lineno">  593</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0;i&lt;N_var;i++){</div><div class="line"><a name="l00594"></a><span class="lineno">  594</span>&#160;        g[i] = H(0,i);            </div><div class="line"><a name="l00595"></a><span class="lineno">  595</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 0;j&lt;N_var;j++)</div><div class="line"><a name="l00596"></a><span class="lineno">  596</span>&#160;            H_qp[j*N_var+i] = 2*Q(i,j);</div><div class="line"><a name="l00597"></a><span class="lineno">  597</span>&#160;    }</div><div class="line"><a name="l00598"></a><span class="lineno">  598</span>&#160;    </div><div class="line"><a name="l00599"></a><span class="lineno">  599</span>&#160;    <span class="comment">// eq constraints</span></div><div class="line"><a name="l00600"></a><span class="lineno">  600</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> r = 0; r&lt;N_eq_const;r++){</div><div class="line"><a name="l00601"></a><span class="lineno">  601</span>&#160;        lbA[r] = beq(r);</div><div class="line"><a name="l00602"></a><span class="lineno">  602</span>&#160;        ubA[r] = beq(r);</div><div class="line"><a name="l00603"></a><span class="lineno">  603</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> c= 0; c&lt;N_var;c++)</div><div class="line"><a name="l00604"></a><span class="lineno">  604</span>&#160;            A[r*N_var + c] = Aeq(r,c);</div><div class="line"><a name="l00605"></a><span class="lineno">  605</span>&#160;    }</div><div class="line"><a name="l00606"></a><span class="lineno">  606</span>&#160;</div><div class="line"><a name="l00607"></a><span class="lineno">  607</span>&#160;    <span class="comment">// ineq constraints</span></div><div class="line"><a name="l00608"></a><span class="lineno">  608</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> r = 0; r&lt;N_ineq_const;r++){</div><div class="line"><a name="l00609"></a><span class="lineno">  609</span>&#160;        lbA[N_eq_const+r] = -1<a class="code" href="namespace__setup__util.html#acdce690b925de33d6249bbbfa1109d61">e</a>+8;</div><div class="line"><a name="l00610"></a><span class="lineno">  610</span>&#160;        ubA[N_eq_const+r] = bineq(r);</div><div class="line"><a name="l00611"></a><span class="lineno">  611</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> c= 0; c&lt;N_var;c++)</div><div class="line"><a name="l00612"></a><span class="lineno">  612</span>&#160;            A[(r+N_eq_const)*N_var + c] = Aineq(r,c);</div><div class="line"><a name="l00613"></a><span class="lineno">  613</span>&#160;    }    </div><div class="line"><a name="l00614"></a><span class="lineno">  614</span>&#160;</div><div class="line"><a name="l00615"></a><span class="lineno">  615</span>&#160;    int_t nWSR = 2000;</div><div class="line"><a name="l00616"></a><span class="lineno">  616</span>&#160;   </div><div class="line"><a name="l00617"></a><span class="lineno">  617</span>&#160;    QProblem qp_obj(N_var,N_const,HST_SEMIDEF);</div><div class="line"><a name="l00618"></a><span class="lineno">  618</span>&#160;    std::cout&lt;&lt;<span class="stringliteral">&quot;hessian type: &quot;</span>&lt;&lt;qp_obj.getHessianType()&lt;&lt;endl;</div><div class="line"><a name="l00619"></a><span class="lineno">  619</span>&#160;    Options options;</div><div class="line"><a name="l00620"></a><span class="lineno">  620</span>&#160;    options.printLevel = PL_LOW;</div><div class="line"><a name="l00621"></a><span class="lineno">  621</span>&#160;    qp_obj.setOptions(options);</div><div class="line"><a name="l00622"></a><span class="lineno">  622</span>&#160;    qp_obj.init(H_qp,g,A,NULL,NULL,lbA,ubA,nWSR);</div><div class="line"><a name="l00623"></a><span class="lineno">  623</span>&#160;    <span class="keywordflow">if</span>(qp_obj.isInfeasible()){</div><div class="line"><a name="l00624"></a><span class="lineno">  624</span>&#160;        cout&lt;&lt;<span class="stringliteral">&quot;[QP solver] warning: problem is infeasible. &quot;</span>&lt;&lt;endl;</div><div class="line"><a name="l00625"></a><span class="lineno">  625</span>&#160;        is_ok = <span class="keyword">false</span>;</div><div class="line"><a name="l00626"></a><span class="lineno">  626</span>&#160;    }</div><div class="line"><a name="l00627"></a><span class="lineno">  627</span>&#160;    real_t xOpt[N_var];</div><div class="line"><a name="l00628"></a><span class="lineno">  628</span>&#160;    qp_obj.getPrimalSolution(xOpt);</div><div class="line"><a name="l00629"></a><span class="lineno">  629</span>&#160;</div><div class="line"><a name="l00630"></a><span class="lineno">  630</span>&#160;    <span class="keywordflow">if</span>(not qp_obj.isSolved()){</div><div class="line"><a name="l00631"></a><span class="lineno">  631</span>&#160;        cout&lt;&lt;<span class="stringliteral">&quot;[QP solver] quadratic programming has not been solved &quot;</span>&lt;&lt;endl;</div><div class="line"><a name="l00632"></a><span class="lineno">  632</span>&#160;        is_ok = <span class="keyword">false</span>;</div><div class="line"><a name="l00633"></a><span class="lineno">  633</span>&#160;    }</div><div class="line"><a name="l00634"></a><span class="lineno">  634</span>&#160;    </div><div class="line"><a name="l00635"></a><span class="lineno">  635</span>&#160;    VectorXd sol(N_var);</div><div class="line"><a name="l00636"></a><span class="lineno">  636</span>&#160;</div><div class="line"><a name="l00637"></a><span class="lineno">  637</span>&#160;    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> n = 0; n&lt;N_var;n++)</div><div class="line"><a name="l00638"></a><span class="lineno">  638</span>&#160;        sol(n) = xOpt[n];</div><div class="line"><a name="l00639"></a><span class="lineno">  639</span>&#160;    <span class="keywordflow">return</span> sol;        </div><div class="line"><a name="l00640"></a><span class="lineno">  640</span>&#160;}</div><div class="line"><a name="l00641"></a><span class="lineno">  641</span>&#160;</div><div class="line"><a name="l00642"></a><span class="lineno">  642</span>&#160;</div><div class="line"><a name="l00643"></a><span class="lineno"><a class="line" href="class_path_planner.html#a35e8872f5ffc5fc3227dc45f9f969a85">  643</a></span>&#160;<a class="code" href="class_poly_spline.html">PolySpline</a> <a class="code" href="class_path_planner.html#a35e8872f5ffc5fc3227dc45f9f969a85">PathPlanner::get_solution</a>(VectorXd sol,<span class="keywordtype">int</span> poly_order,<span class="keywordtype">int</span> n_seg ){</div><div class="line"><a name="l00644"></a><span class="lineno">  644</span>&#160;            </div><div class="line"><a name="l00645"></a><span class="lineno">  645</span>&#160;    <span class="comment">// this initialization of spline object </span></div><div class="line"><a name="l00646"></a><span class="lineno">  646</span>&#160;    <a class="code" href="class_poly_spline.html">PolySpline</a> polySpline;</div><div class="line"><a name="l00647"></a><span class="lineno">  647</span>&#160;    </div><div class="line"><a name="l00648"></a><span class="lineno">  648</span>&#160;    polySpline.n_seg=n_seg;</div><div class="line"><a name="l00649"></a><span class="lineno">  649</span>&#160;    <span class="comment">// polySpline.knot_time.reserve(n_seg+1);  // let&#39;s insert the time later outside of this fucntion </span></div><div class="line"><a name="l00650"></a><span class="lineno">  650</span>&#160;    polySpline.poly_coeff.reserve(n_seg);  <span class="comment">// let&#39;s insert the time later outside of this fucntion </span></div><div class="line"><a name="l00651"></a><span class="lineno">  651</span>&#160;        </div><div class="line"><a name="l00652"></a><span class="lineno">  652</span>&#160;    <span class="keywordflow">for</span>(uint i = 0; i&lt;polySpline.n_seg;i++)</div><div class="line"><a name="l00653"></a><span class="lineno">  653</span>&#160;        polySpline.poly_coeff[i].poly_order=poly_order;</div><div class="line"><a name="l00654"></a><span class="lineno">  654</span>&#160;    <span class="keywordtype">int</span> n_var=n_seg*(poly_order+1);</div><div class="line"><a name="l00655"></a><span class="lineno">  655</span>&#160;</div><div class="line"><a name="l00656"></a><span class="lineno">  656</span>&#160;    <span class="comment">// std::cout&lt;&lt;&quot;[DEBUG] solution:&quot;&lt;&lt;std::endl;</span></div><div class="line"><a name="l00657"></a><span class="lineno">  657</span>&#160;    <span class="comment">// std::cout&lt;&lt;var&lt;&lt;std::endl;</span></div><div class="line"><a name="l00658"></a><span class="lineno">  658</span>&#160;    <span class="comment">// from lowest order 0</span></div><div class="line"><a name="l00659"></a><span class="lineno">  659</span>&#160;    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> n=0;n&lt;n_seg;n++){</div><div class="line"><a name="l00660"></a><span class="lineno">  660</span>&#160;        <a class="code" href="class_poly_coeff.html">PolyCoeff</a> coeff;</div><div class="line"><a name="l00661"></a><span class="lineno">  661</span>&#160;        coeff.coeff.resize(poly_order+1);</div><div class="line"><a name="l00662"></a><span class="lineno">  662</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0;i&lt;poly_order+1;i++){</div><div class="line"><a name="l00663"></a><span class="lineno">  663</span>&#160;            coeff.coeff[i]=sol(n*(poly_order+1)+i); </div><div class="line"><a name="l00664"></a><span class="lineno">  664</span>&#160;            coeff.poly_order = poly_order;         </div><div class="line"><a name="l00665"></a><span class="lineno">  665</span>&#160;        }</div><div class="line"><a name="l00666"></a><span class="lineno">  666</span>&#160;        polySpline.poly_coeff.push_back(coeff);</div><div class="line"><a name="l00667"></a><span class="lineno">  667</span>&#160;    }</div><div class="line"><a name="l00668"></a><span class="lineno">  668</span>&#160;</div><div class="line"><a name="l00669"></a><span class="lineno">  669</span>&#160;    <span class="keywordflow">return</span> polySpline;</div><div class="line"><a name="l00670"></a><span class="lineno">  670</span>&#160;</div><div class="line"><a name="l00671"></a><span class="lineno">  671</span>&#160;}</div><div class="line"><a name="l00672"></a><span class="lineno">  672</span>&#160;</div><div class="line"><a name="l00673"></a><span class="lineno">  673</span>&#160;</div><div class="line"><a name="l00674"></a><span class="lineno">  674</span>&#160;</div><div class="line"><a name="l00675"></a><span class="lineno">  675</span>&#160;<span class="comment">// output size 3x(N+1)</span></div><div class="line"><a name="l00676"></a><span class="lineno"><a class="line" href="class_path_planner.html#a1d6a959095513585bfc10468f7b8d180">  676</a></span>&#160;<a class="code" href="struct_constraint.html">Constraint</a> <a class="code" href="class_path_planner.html#a1d6a959095513585bfc10468f7b8d180">PathPlanner::get_init_constraint_mat</a>(<span class="keywordtype">double</span> x0,<span class="keywordtype">double</span> v0,<span class="keywordtype">double</span> a0,<a class="code" href="struct_traj_gen_opts.html">TrajGenOpts</a> opt){</div><div class="line"><a name="l00677"></a><span class="lineno">  677</span>&#160;    <span class="keywordtype">int</span> poly_order = opt.<a class="code" href="struct_traj_gen_opts.html#a538985230ef4a8cd7763c91d11bebbaf">poly_order</a>;</div><div class="line"><a name="l00678"></a><span class="lineno">  678</span>&#160;    MatrixXd Aeq0(3,poly_order+1);</div><div class="line"><a name="l00679"></a><span class="lineno">  679</span>&#160;    MatrixXd beq0(3,1);</div><div class="line"><a name="l00680"></a><span class="lineno">  680</span>&#160;    Aeq0.setZero();</div><div class="line"><a name="l00681"></a><span class="lineno">  681</span>&#160;    beq0.setZero();</div><div class="line"><a name="l00682"></a><span class="lineno">  682</span>&#160;    <span class="keywordtype">double</span> dt_arbitrary = 1;</div><div class="line"><a name="l00683"></a><span class="lineno">  683</span>&#160;</div><div class="line"><a name="l00684"></a><span class="lineno">  684</span>&#160;    <span class="comment">// Aeq0.row(0) = t_vec(poly_order,0,0).transpose()*time_scailing_mat(dt_arbitrary,poly_order);</span></div><div class="line"><a name="l00685"></a><span class="lineno">  685</span>&#160;    <span class="comment">// Aeq0.row(1) = t_vec(poly_order,0,1).transpose()*time_scailing_mat(dt_arbitrary,poly_order)/dt_arbitrary;</span></div><div class="line"><a name="l00686"></a><span class="lineno">  686</span>&#160;    <span class="comment">// Aeq0.row(2) = t_vec(poly_order,0,2).transpose()*time_scailing_mat(dt_arbitrary,poly_order)/pow(dt_arbitrary,2);</span></div><div class="line"><a name="l00687"></a><span class="lineno">  687</span>&#160;</div><div class="line"><a name="l00688"></a><span class="lineno">  688</span>&#160;    Aeq0.row(0) = <a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,0,0).transpose();</div><div class="line"><a name="l00689"></a><span class="lineno">  689</span>&#160;    Aeq0.row(1) = <a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,0,1).transpose()/dt_arbitrary;</div><div class="line"><a name="l00690"></a><span class="lineno">  690</span>&#160;    Aeq0.row(2) = <a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,0,2).transpose()/pow(dt_arbitrary,2);</div><div class="line"><a name="l00691"></a><span class="lineno">  691</span>&#160;</div><div class="line"><a name="l00692"></a><span class="lineno">  692</span>&#160;    beq0(0) = x0;</div><div class="line"><a name="l00693"></a><span class="lineno">  693</span>&#160;    beq0(1) = v0;</div><div class="line"><a name="l00694"></a><span class="lineno">  694</span>&#160;    beq0(2) = a0;</div><div class="line"><a name="l00695"></a><span class="lineno">  695</span>&#160;    </div><div class="line"><a name="l00696"></a><span class="lineno">  696</span>&#160;    <a class="code" href="struct_constraint.html">Constraint</a> constraint;</div><div class="line"><a name="l00697"></a><span class="lineno">  697</span>&#160;    constraint.<a class="code" href="struct_constraint.html#a95639f38cb8a18c60a96e629160a9571">A</a> = Aeq0;</div><div class="line"><a name="l00698"></a><span class="lineno">  698</span>&#160;    constraint.<a class="code" href="struct_constraint.html#ab22b0463922cee8c89b0443ed611d072">b</a> = beq0;</div><div class="line"><a name="l00699"></a><span class="lineno">  699</span>&#160;    <span class="keywordflow">return</span> constraint;</div><div class="line"><a name="l00700"></a><span class="lineno">  700</span>&#160;}</div><div class="line"><a name="l00701"></a><span class="lineno">  701</span>&#160;</div><div class="line"><a name="l00702"></a><span class="lineno">  702</span>&#160;</div><div class="line"><a name="l00703"></a><span class="lineno"><a class="line" href="class_path_planner.html#a5c9fd37284fa20b93f933ce53c932fc6">  703</a></span>&#160;visualization_msgs::Marker <a class="code" href="class_path_planner.html#a5c9fd37284fa20b93f933ce53c932fc6">PathPlanner::get_knots_marker</a>(){</div><div class="line"><a name="l00704"></a><span class="lineno">  704</span>&#160; </div><div class="line"><a name="l00705"></a><span class="lineno">  705</span>&#160;    visualization_msgs::Marker knots_marker;</div><div class="line"><a name="l00706"></a><span class="lineno">  706</span>&#160;</div><div class="line"><a name="l00707"></a><span class="lineno">  707</span>&#160;    knots_marker.ns = <span class="stringliteral">&quot;knots&quot;</span>;</div><div class="line"><a name="l00708"></a><span class="lineno">  708</span>&#160;    knots_marker.id = 0;</div><div class="line"><a name="l00709"></a><span class="lineno">  709</span>&#160;    knots_marker.type = visualization_msgs::Marker::SPHERE_LIST;</div><div class="line"><a name="l00710"></a><span class="lineno">  710</span>&#160;    knots_marker.color.r = 10.0/255.0;</div><div class="line"><a name="l00711"></a><span class="lineno">  711</span>&#160;    knots_marker.color.g = 50.0/255.0;</div><div class="line"><a name="l00712"></a><span class="lineno">  712</span>&#160;    knots_marker.color.b = 1.0;</div><div class="line"><a name="l00713"></a><span class="lineno">  713</span>&#160;    knots_marker.color.a = 0.8;</div><div class="line"><a name="l00714"></a><span class="lineno">  714</span>&#160;    knots_marker.pose.orientation.w = 1.0;</div><div class="line"><a name="l00715"></a><span class="lineno">  715</span>&#160;    <span class="keywordtype">double</span> scale = 0.2; </div><div class="line"><a name="l00716"></a><span class="lineno">  716</span>&#160;    knots_marker.scale.x = scale;</div><div class="line"><a name="l00717"></a><span class="lineno">  717</span>&#160;    knots_marker.scale.y = scale;</div><div class="line"><a name="l00718"></a><span class="lineno">  718</span>&#160;    knots_marker.scale.z = scale;       </div><div class="line"><a name="l00719"></a><span class="lineno">  719</span>&#160;    </div><div class="line"><a name="l00720"></a><span class="lineno">  720</span>&#160;    <span class="keywordflow">for</span> (<span class="keyword">auto</span> it = spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>.begin(); it&lt;spline_xyz.<a class="code" href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">knot_time</a>.end();it++)</div><div class="line"><a name="l00721"></a><span class="lineno">  721</span>&#160;        knots_marker.points.push_back(<a class="code" href="class_path_planner.html#aa12888c52d7cb2d429c3782060b9b45e">point_eval_spline</a>(*it));</div><div class="line"><a name="l00722"></a><span class="lineno">  722</span>&#160;</div><div class="line"><a name="l00723"></a><span class="lineno">  723</span>&#160;    <span class="keywordflow">return</span> knots_marker;</div><div class="line"><a name="l00724"></a><span class="lineno">  724</span>&#160;}</div><div class="line"><a name="l00725"></a><span class="lineno">  725</span>&#160;</div><div class="line"><a name="l00726"></a><span class="lineno">  726</span>&#160;<span class="comment">// output size 3x(2(N+1))</span></div><div class="line"><a name="l00727"></a><span class="lineno"><a class="line" href="class_path_planner.html#aff99e738bd54babad38867d7713e5a51">  727</a></span>&#160;<a class="code" href="struct_constraint.html">Constraint</a> <a class="code" href="class_path_planner.html#aff99e738bd54babad38867d7713e5a51">PathPlanner::get_continuity_constraint_mat</a>(<span class="keywordtype">double</span> dt1,<span class="keywordtype">double</span> dt2,<a class="code" href="struct_traj_gen_opts.html">TrajGenOpts</a> opt){</div><div class="line"><a name="l00728"></a><span class="lineno">  728</span>&#160;    </div><div class="line"><a name="l00729"></a><span class="lineno">  729</span>&#160;    <span class="keywordtype">int</span> N_constraint = opt.<a class="code" href="struct_traj_gen_opts.html#ad8937f645e1154dc52978bd6592ce836">objective_derivative</a>;</div><div class="line"><a name="l00730"></a><span class="lineno">  730</span>&#160;    <span class="keywordtype">int</span> poly_order = opt.<a class="code" href="struct_traj_gen_opts.html#a538985230ef4a8cd7763c91d11bebbaf">poly_order</a>;</div><div class="line"><a name="l00731"></a><span class="lineno">  731</span>&#160;    MatrixXd Aeq(N_constraint,2*(poly_order+1));</div><div class="line"><a name="l00732"></a><span class="lineno">  732</span>&#160;    MatrixXd beq(N_constraint,1); beq.setZero();   </div><div class="line"><a name="l00733"></a><span class="lineno">  733</span>&#160;    Aeq.setZero();</div><div class="line"><a name="l00734"></a><span class="lineno">  734</span>&#160;    beq.setZero();</div><div class="line"><a name="l00735"></a><span class="lineno">  735</span>&#160;    MatrixXd D1 = <a class="code" href="_poly_traj_gen_8cpp.html#a5a2e38cc8bf6ae9ab0395994b68e52ae">time_scailing_mat</a>(dt1,poly_order);</div><div class="line"><a name="l00736"></a><span class="lineno">  736</span>&#160;    MatrixXd D2 = <a class="code" href="_poly_traj_gen_8cpp.html#a5a2e38cc8bf6ae9ab0395994b68e52ae">time_scailing_mat</a>(dt2,poly_order);</div><div class="line"><a name="l00737"></a><span class="lineno">  737</span>&#160;</div><div class="line"><a name="l00738"></a><span class="lineno">  738</span>&#160;    <span class="comment">// 0th order         </span></div><div class="line"><a name="l00739"></a><span class="lineno">  739</span>&#160;    <span class="comment">// Aeq. block(0,0,1,poly_order+1) = t_vec(poly_order,1,0).transpose()*D1;</span></div><div class="line"><a name="l00740"></a><span class="lineno">  740</span>&#160;    <span class="comment">// Aeq.block(0,poly_order+1,1,poly_order+1) = -t_vec(poly_order,0,0).transpose()*D2;</span></div><div class="line"><a name="l00741"></a><span class="lineno">  741</span>&#160;</div><div class="line"><a name="l00742"></a><span class="lineno">  742</span>&#160;    Aeq.block(0,0,1,poly_order+1) = <a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,1,0).transpose();</div><div class="line"><a name="l00743"></a><span class="lineno">  743</span>&#160;    Aeq.block(0,poly_order+1,1,poly_order+1) = -<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,0,0).transpose();</div><div class="line"><a name="l00744"></a><span class="lineno">  744</span>&#160;</div><div class="line"><a name="l00745"></a><span class="lineno">  745</span>&#160;</div><div class="line"><a name="l00746"></a><span class="lineno">  746</span>&#160;    <span class="comment">// 1st order</span></div><div class="line"><a name="l00747"></a><span class="lineno">  747</span>&#160;    <span class="comment">// Aeq.block(1,0,1,poly_order+1) = t_vec(poly_order,1,1).transpose()*D1*dt2;</span></div><div class="line"><a name="l00748"></a><span class="lineno">  748</span>&#160;    <span class="comment">// Aeq.block(1,poly_order+1,1,poly_order+1) = -t_vec(poly_order,0,1).transpose()*D2*dt1; </span></div><div class="line"><a name="l00749"></a><span class="lineno">  749</span>&#160;</div><div class="line"><a name="l00750"></a><span class="lineno">  750</span>&#160;    Aeq.block(1,0,1,poly_order+1) = <a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,1,1).transpose()*dt2;</div><div class="line"><a name="l00751"></a><span class="lineno">  751</span>&#160;    Aeq.block(1,poly_order+1,1,poly_order+1) = -<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,0,1).transpose()*dt1; </div><div class="line"><a name="l00752"></a><span class="lineno">  752</span>&#160;</div><div class="line"><a name="l00753"></a><span class="lineno">  753</span>&#160;</div><div class="line"><a name="l00754"></a><span class="lineno">  754</span>&#160;    <span class="comment">// 2nd order</span></div><div class="line"><a name="l00755"></a><span class="lineno">  755</span>&#160;    Aeq.block(2,0,1,poly_order+1) = <a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,1,2).transpose()*pow(dt2,2);</div><div class="line"><a name="l00756"></a><span class="lineno">  756</span>&#160;    Aeq.block(2,poly_order+1,1,poly_order+1) = -<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,0,2).transpose()*pow(dt1,2); </div><div class="line"><a name="l00757"></a><span class="lineno">  757</span>&#160;</div><div class="line"><a name="l00758"></a><span class="lineno">  758</span>&#160;     <span class="keywordflow">if</span> (N_constraint == 4){    </div><div class="line"><a name="l00759"></a><span class="lineno">  759</span>&#160;        <span class="comment">// 3rd order</span></div><div class="line"><a name="l00760"></a><span class="lineno">  760</span>&#160;        Aeq.block(3,0,1,poly_order+1) = <a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,1,3).transpose()*pow(dt2,3);</div><div class="line"><a name="l00761"></a><span class="lineno">  761</span>&#160;        Aeq.block(3,poly_order+1,1,poly_order+1) = -<a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(poly_order,0,3).transpose()*pow(dt1,3); </div><div class="line"><a name="l00762"></a><span class="lineno">  762</span>&#160;    }</div><div class="line"><a name="l00763"></a><span class="lineno">  763</span>&#160;</div><div class="line"><a name="l00764"></a><span class="lineno">  764</span>&#160;    <a class="code" href="struct_constraint.html">Constraint</a> constraint;</div><div class="line"><a name="l00765"></a><span class="lineno">  765</span>&#160;    constraint.<a class="code" href="struct_constraint.html#a95639f38cb8a18c60a96e629160a9571">A</a> = Aeq;</div><div class="line"><a name="l00766"></a><span class="lineno">  766</span>&#160;    constraint.<a class="code" href="struct_constraint.html#ab22b0463922cee8c89b0443ed611d072">b</a> = beq;</div><div class="line"><a name="l00767"></a><span class="lineno">  767</span>&#160;    </div><div class="line"><a name="l00768"></a><span class="lineno">  768</span>&#160;    <span class="keywordflow">return</span> constraint;</div><div class="line"><a name="l00769"></a><span class="lineno">  769</span>&#160;}</div><div class="line"><a name="l00770"></a><span class="lineno">  770</span>&#160;</div><div class="line"><a name="l00771"></a><span class="lineno">  771</span>&#160;</div><div class="line"><a name="l00772"></a><span class="lineno">  772</span>&#160;</div><div class="line"><a name="l00773"></a><span class="lineno">  773</span>&#160;</div><div class="line"><a name="l00774"></a><span class="lineno"><a class="line" href="_poly_traj_gen_8cpp.html#a5a2e38cc8bf6ae9ab0395994b68e52ae">  774</a></span>&#160;MatrixXd <a class="code" href="_poly_traj_gen_8cpp.html#a5a2e38cc8bf6ae9ab0395994b68e52ae">time_scailing_mat</a>(<span class="keywordtype">double</span> dt, <span class="keywordtype">int</span> poly_order) {</div><div class="line"><a name="l00775"></a><span class="lineno">  775</span>&#160;    MatrixXd D(poly_order+1,poly_order+1);</div><div class="line"><a name="l00776"></a><span class="lineno">  776</span>&#160;    D.setZero();</div><div class="line"><a name="l00777"></a><span class="lineno">  777</span>&#160;</div><div class="line"><a name="l00778"></a><span class="lineno">  778</span>&#160;    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0;i&lt;poly_order+1;i++)</div><div class="line"><a name="l00779"></a><span class="lineno">  779</span>&#160;        D.coeffRef(i,i)=pow(dt,i);</div><div class="line"><a name="l00780"></a><span class="lineno">  780</span>&#160;    <span class="keywordflow">return</span> D;</div><div class="line"><a name="l00781"></a><span class="lineno">  781</span>&#160;}</div><div class="line"><a name="l00782"></a><span class="lineno">  782</span>&#160;</div><div class="line"><a name="l00783"></a><span class="lineno">  783</span>&#160;</div><div class="line"><a name="l00784"></a><span class="lineno"><a class="line" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">  784</a></span>&#160;VectorXd <a class="code" href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a>(<span class="keywordtype">int</span> poly_order, <span class="keywordtype">double</span> t, <span class="keywordtype">int</span> n_diff) {</div><div class="line"><a name="l00785"></a><span class="lineno">  785</span>&#160;    VectorXd vec(poly_order+1);</div><div class="line"><a name="l00786"></a><span class="lineno">  786</span>&#160;    vec.setZero();</div><div class="line"><a name="l00787"></a><span class="lineno">  787</span>&#160;    <span class="keywordflow">switch</span>(n_diff){</div><div class="line"><a name="l00788"></a><span class="lineno">  788</span>&#160;        <span class="keywordflow">case</span> 0:</div><div class="line"><a name="l00789"></a><span class="lineno">  789</span>&#160;            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0;i&lt;poly_order+1;i++)</div><div class="line"><a name="l00790"></a><span class="lineno">  790</span>&#160;                vec.coeffRef(i)=pow(t,i);</div><div class="line"><a name="l00791"></a><span class="lineno">  791</span>&#160;            <span class="keywordflow">break</span>;</div><div class="line"><a name="l00792"></a><span class="lineno">  792</span>&#160;        <span class="keywordflow">case</span> 1:</div><div class="line"><a name="l00793"></a><span class="lineno">  793</span>&#160;            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=1;i&lt;poly_order+1;i++)</div><div class="line"><a name="l00794"></a><span class="lineno">  794</span>&#160;                vec.coeffRef(i)=i*pow(t,i-1);</div><div class="line"><a name="l00795"></a><span class="lineno">  795</span>&#160;</div><div class="line"><a name="l00796"></a><span class="lineno">  796</span>&#160;            <span class="keywordflow">break</span>;</div><div class="line"><a name="l00797"></a><span class="lineno">  797</span>&#160;        <span class="keywordflow">case</span> 2:</div><div class="line"><a name="l00798"></a><span class="lineno">  798</span>&#160;            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=2;i&lt;poly_order+1;i++)</div><div class="line"><a name="l00799"></a><span class="lineno">  799</span>&#160;                vec.coeffRef(i)=i*(i-1)*pow(t,i-2);</div><div class="line"><a name="l00800"></a><span class="lineno">  800</span>&#160;            <span class="keywordflow">break</span>;</div><div class="line"><a name="l00801"></a><span class="lineno">  801</span>&#160;</div><div class="line"><a name="l00802"></a><span class="lineno">  802</span>&#160;        <span class="keywordflow">case</span> 3:</div><div class="line"><a name="l00803"></a><span class="lineno">  803</span>&#160;            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=3;i&lt;poly_order+1;i++)</div><div class="line"><a name="l00804"></a><span class="lineno">  804</span>&#160;                vec.coeffRef(i)=i*(i-1)*(i-2)*pow(t,i-3);</div><div class="line"><a name="l00805"></a><span class="lineno">  805</span>&#160;            <span class="keywordflow">break</span>;</div><div class="line"><a name="l00806"></a><span class="lineno">  806</span>&#160;    }</div><div class="line"><a name="l00807"></a><span class="lineno">  807</span>&#160;</div><div class="line"><a name="l00808"></a><span class="lineno">  808</span>&#160;    <span class="keywordflow">return</span> vec;</div><div class="line"><a name="l00809"></a><span class="lineno">  809</span>&#160;}</div><div class="line"><a name="l00810"></a><span class="lineno">  810</span>&#160;</div><div class="line"><a name="l00811"></a><span class="lineno">  811</span>&#160;</div><div class="line"><a name="l00812"></a><span class="lineno"><a class="line" href="_poly_traj_gen_8cpp.html#a0b792e87b38c7882b087726a82eaa72e">  812</a></span>&#160; MatrixXd <a class="code" href="_poly_traj_gen_8cpp.html#a0b792e87b38c7882b087726a82eaa72e">integral_jerk_squared</a>(<span class="keywordtype">int</span> poly_order){</div><div class="line"><a name="l00813"></a><span class="lineno">  813</span>&#160;    <span class="comment">// this is ingeral of jerk matrix from 0 to 1 given polynomial order</span></div><div class="line"><a name="l00814"></a><span class="lineno">  814</span>&#160;     <span class="keywordtype">int</span> n=poly_order;</div><div class="line"><a name="l00815"></a><span class="lineno">  815</span>&#160;    MatrixXd Qj(n+1,n+1);</div><div class="line"><a name="l00816"></a><span class="lineno">  816</span>&#160;    Qj.setZero();</div><div class="line"><a name="l00817"></a><span class="lineno">  817</span>&#160;</div><div class="line"><a name="l00818"></a><span class="lineno">  818</span>&#160;    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=3;i&lt;n+1;i++)</div><div class="line"><a name="l00819"></a><span class="lineno">  819</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j=3;j&lt;n+1;j++)</div><div class="line"><a name="l00820"></a><span class="lineno">  820</span>&#160;            <span class="keywordflow">if</span>(i==3 and j==3)</div><div class="line"><a name="l00821"></a><span class="lineno">  821</span>&#160;                Qj.coeffRef(i,j)=i*(i-1)*(i-2)*j*(j-1)*(j-2);</div><div class="line"><a name="l00822"></a><span class="lineno">  822</span>&#160;            <span class="keywordflow">else</span></div><div class="line"><a name="l00823"></a><span class="lineno">  823</span>&#160;                Qj.coeffRef(i,j)=i*(i-1)*(i-2)*j*(j-1)*(j-2)/(i+j-5);</div><div class="line"><a name="l00824"></a><span class="lineno">  824</span>&#160;</div><div class="line"><a name="l00825"></a><span class="lineno">  825</span>&#160;</div><div class="line"><a name="l00826"></a><span class="lineno">  826</span>&#160;    <span class="keywordflow">return</span> Qj;</div><div class="line"><a name="l00827"></a><span class="lineno">  827</span>&#160;}</div><div class="line"><a name="l00828"></a><span class="lineno">  828</span>&#160;</div><div class="line"><a name="l00829"></a><span class="lineno"><a class="line" href="_poly_traj_gen_8cpp.html#a98c8e48d65c22d6da6b6b82242586f15">  829</a></span>&#160; MatrixXd <a class="code" href="_poly_traj_gen_8cpp.html#a98c8e48d65c22d6da6b6b82242586f15">integral_snap_squared</a>(<span class="keywordtype">int</span> poly_order){</div><div class="line"><a name="l00830"></a><span class="lineno">  830</span>&#160;    <span class="comment">// this is ingeral of jerk matrix from 0 to 1 given polynomial order</span></div><div class="line"><a name="l00831"></a><span class="lineno">  831</span>&#160;     <span class="keywordtype">int</span> n=poly_order;</div><div class="line"><a name="l00832"></a><span class="lineno">  832</span>&#160;    MatrixXd Qj(n+1,n+1);</div><div class="line"><a name="l00833"></a><span class="lineno">  833</span>&#160;    Qj.setZero();</div><div class="line"><a name="l00834"></a><span class="lineno">  834</span>&#160;</div><div class="line"><a name="l00835"></a><span class="lineno">  835</span>&#160;    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=4;i&lt;n+1;i++)</div><div class="line"><a name="l00836"></a><span class="lineno">  836</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j=4;j&lt;n+1;j++)</div><div class="line"><a name="l00837"></a><span class="lineno">  837</span>&#160;            <span class="keywordflow">if</span>(i==4 and j==4)</div><div class="line"><a name="l00838"></a><span class="lineno">  838</span>&#160;                Qj.coeffRef(i,j)=i*(i-1)*(i-2)*(i-3)*j*(j-1)*(j-2)*(j-3);</div><div class="line"><a name="l00839"></a><span class="lineno">  839</span>&#160;            <span class="keywordflow">else</span></div><div class="line"><a name="l00840"></a><span class="lineno">  840</span>&#160;                Qj.coeffRef(i,j)=i*(i-1)*(i-2)*(i-3)*j*(j-1)*(j-2)*(j-3)/(i+j-7);</div><div class="line"><a name="l00841"></a><span class="lineno">  841</span>&#160;</div><div class="line"><a name="l00842"></a><span class="lineno">  842</span>&#160;</div><div class="line"><a name="l00843"></a><span class="lineno">  843</span>&#160;    <span class="keywordflow">return</span> Qj;</div><div class="line"><a name="l00844"></a><span class="lineno">  844</span>&#160;}</div><div class="line"><a name="l00845"></a><span class="lineno">  845</span>&#160;</div><div class="line"><a name="l00846"></a><span class="lineno"><a class="line" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">  846</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a>(MatrixXd &amp; mat,MatrixXd mat_sub){</div><div class="line"><a name="l00847"></a><span class="lineno">  847</span>&#160;    <span class="keywordtype">int</span> orig_row_size = mat.rows();</div><div class="line"><a name="l00848"></a><span class="lineno">  848</span>&#160;    mat.conservativeResize(mat.rows()+mat_sub.rows(),mat.cols());</div><div class="line"><a name="l00849"></a><span class="lineno">  849</span>&#160;    mat.block(orig_row_size,0,mat_sub.rows(),mat.cols()) = mat_sub;</div><div class="line"><a name="l00850"></a><span class="lineno">  850</span>&#160;</div><div class="line"><a name="l00851"></a><span class="lineno">  851</span>&#160;}</div><div class="line"><a name="l00852"></a><span class="lineno">  852</span>&#160;</div><div class="line"><a name="l00853"></a><span class="lineno">  853</span>&#160;</div><div class="line"><a name="l00854"></a><span class="lineno">  854</span>&#160;</div><div class="line"><a name="l00855"></a><span class="lineno"><a class="line" href="_poly_traj_gen_8cpp.html#a17e17da0f42201d595257679c8ae6170">  855</a></span>&#160;<span class="keywordtype">int</span> <a class="code" href="_poly_traj_gen_8cpp.html#a17e17da0f42201d595257679c8ae6170">find_spline_interval</a>(<span class="keyword">const</span> vector&lt;double&gt;&amp; ts,<span class="keywordtype">double</span> t_eval) {</div><div class="line"><a name="l00856"></a><span class="lineno">  856</span>&#160;</div><div class="line"><a name="l00857"></a><span class="lineno">  857</span>&#160;</div><div class="line"><a name="l00858"></a><span class="lineno">  858</span>&#160;    <span class="keywordtype">int</span> idx=-1;</div><div class="line"><a name="l00859"></a><span class="lineno">  859</span>&#160;    </div><div class="line"><a name="l00860"></a><span class="lineno">  860</span>&#160;    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0;i&lt;ts.size()-1;i++)</div><div class="line"><a name="l00861"></a><span class="lineno">  861</span>&#160;        <span class="keywordflow">if</span>(ts[i]&lt;=t_eval &amp;&amp; ts[i+1]&gt;t_eval)</div><div class="line"><a name="l00862"></a><span class="lineno">  862</span>&#160;            idx=i;</div><div class="line"><a name="l00863"></a><span class="lineno">  863</span>&#160;    <span class="keywordflow">if</span> (t_eval &gt;= ts.back())</div><div class="line"><a name="l00864"></a><span class="lineno">  864</span>&#160;        idx = ts.size()-2;</div><div class="line"><a name="l00865"></a><span class="lineno">  865</span>&#160;</div><div class="line"><a name="l00866"></a><span class="lineno">  866</span>&#160;    <span class="keywordflow">return</span> idx;</div><div class="line"><a name="l00867"></a><span class="lineno">  867</span>&#160;</div><div class="line"><a name="l00868"></a><span class="lineno">  868</span>&#160;    <span class="comment">// if idx == -1, then could not find</span></div><div class="line"><a name="l00869"></a><span class="lineno">  869</span>&#160;}</div><div class="ttc" id="_poly_traj_gen_8cpp_html_a98c8e48d65c22d6da6b6b82242586f15"><div class="ttname"><a href="_poly_traj_gen_8cpp.html#a98c8e48d65c22d6da6b6b82242586f15">integral_snap_squared</a></div><div class="ttdeci">MatrixXd integral_snap_squared(int poly_order)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00829">PolyTrajGen.cpp:829</a></div></div>
<div class="ttc" id="struct_q_p__form__xyz_html_a243478dd3ac49ecf0d6fceffbcdf408b"><div class="ttname"><a href="struct_q_p__form__xyz.html#a243478dd3ac49ecf0d6fceffbcdf408b">QP_form_xyz::z</a></div><div class="ttdeci">QP_form z</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00102">PolyTrajGen.h:102</a></div></div>
<div class="ttc" id="struct_q_p__form_html_a3671f01cebe0e9acdb6ce105b6eff780"><div class="ttname"><a href="struct_q_p__form.html#a3671f01cebe0e9acdb6ce105b6eff780">QP_form::beq</a></div><div class="ttdeci">MatrixXd beq</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00095">PolyTrajGen.h:95</a></div></div>
<div class="ttc" id="struct_traj_gen_opts_html"><div class="ttname"><a href="struct_traj_gen_opts.html">TrajGenOpts</a></div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00063">PolyTrajGen.h:63</a></div></div>
<div class="ttc" id="struct_constraint_html"><div class="ttname"><a href="struct_constraint.html">Constraint</a></div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00082">PolyTrajGen.h:82</a></div></div>
<div class="ttc" id="struct_q_p__form__xyz_html_a3e1c8fbfd5a4650fb45fc0ca14c2bcb6"><div class="ttname"><a href="struct_q_p__form__xyz.html#a3e1c8fbfd5a4650fb45fc0ca14c2bcb6">QP_form_xyz::y</a></div><div class="ttdeci">QP_form y</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00101">PolyTrajGen.h:101</a></div></div>
<div class="ttc" id="_poly_traj_gen_8cpp_html_a0b792e87b38c7882b087726a82eaa72e"><div class="ttname"><a href="_poly_traj_gen_8cpp.html#a0b792e87b38c7882b087726a82eaa72e">integral_jerk_squared</a></div><div class="ttdeci">MatrixXd integral_jerk_squared(int poly_order)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00812">PolyTrajGen.cpp:812</a></div></div>
<div class="ttc" id="class_path_planner_html_a197058bceddd7a22cd23c74239562754"><div class="ttname"><a href="class_path_planner.html#a197058bceddd7a22cd23c74239562754">PathPlanner::path_gen</a></div><div class="ttdeci">void path_gen(const TimeSeries &amp;knots, const nav_msgs::Path &amp;waypoints, const geometry_msgs::Twist &amp;v0, const geometry_msgs::Twist &amp;a0, TrajGenOpts opt)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00054">PolyTrajGen.cpp:54</a></div></div>
<div class="ttc" id="struct_q_p__form__xyz_html"><div class="ttname"><a href="struct_q_p__form__xyz.html">QP_form_xyz</a></div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00098">PolyTrajGen.h:98</a></div></div>
<div class="ttc" id="_poly_traj_gen_8cpp_html_a2fbc0abee53f736fe5f5d2a5c04ec723"><div class="ttname"><a href="_poly_traj_gen_8cpp.html#a2fbc0abee53f736fe5f5d2a5c04ec723">row_append</a></div><div class="ttdeci">void row_append(MatrixXd &amp;mat, MatrixXd mat_sub)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00846">PolyTrajGen.cpp:846</a></div></div>
<div class="ttc" id="struct_q_p__form_html_af0fab4bf47d9b4a2598445bd11f446e0"><div class="ttname"><a href="struct_q_p__form.html#af0fab4bf47d9b4a2598445bd11f446e0">QP_form::Aeq</a></div><div class="ttdeci">MatrixXd Aeq</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00094">PolyTrajGen.h:94</a></div></div>
<div class="ttc" id="struct_q_p__form_html"><div class="ttname"><a href="struct_q_p__form.html">QP_form</a></div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00089">PolyTrajGen.h:89</a></div></div>
<div class="ttc" id="class_path_planner_html_a376f30d795cfe0a40f8923f49336f7da"><div class="ttname"><a href="class_path_planner.html#a376f30d795cfe0a40f8923f49336f7da">PathPlanner::PathPlanner</a></div><div class="ttdeci">PathPlanner()</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00004">PolyTrajGen.cpp:4</a></div></div>
<div class="ttc" id="structtraj__gen_1_1_poly_spline_x_y_z___html_a0e2ce289d7bb5fbcfe7e436c3c24b3a6"><div class="ttname"><a href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a0e2ce289d7bb5fbcfe7e436c3c24b3a6">traj_gen::PolySplineXYZ_::spline_x</a></div><div class="ttdeci">_spline_x_type spline_x</div><div class="ttdef"><b>Definition:</b> <a href="_poly_spline_x_y_z_8h_source.html#l00055">PolySplineXYZ.h:55</a></div></div>
<div class="ttc" id="struct_traj_gen_opts_html_aba857cb0ae86cbac99ba5adf9320c66d"><div class="ttname"><a href="struct_traj_gen_opts.html#aba857cb0ae86cbac99ba5adf9320c66d">TrajGenOpts::safe_r</a></div><div class="ttdeci">double safe_r</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00075">PolyTrajGen.h:75</a></div></div>
<div class="ttc" id="_poly_traj_gen_8cpp_html_a5a2e38cc8bf6ae9ab0395994b68e52ae"><div class="ttname"><a href="_poly_traj_gen_8cpp.html#a5a2e38cc8bf6ae9ab0395994b68e52ae">time_scailing_mat</a></div><div class="ttdeci">MatrixXd time_scailing_mat(double dt, int poly_order)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00774">PolyTrajGen.cpp:774</a></div></div>
<div class="ttc" id="struct_constraint_html_a95639f38cb8a18c60a96e629160a9571"><div class="ttname"><a href="struct_constraint.html#a95639f38cb8a18c60a96e629160a9571">Constraint::A</a></div><div class="ttdeci">MatrixXd A</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00084">PolyTrajGen.h:84</a></div></div>
<div class="ttc" id="class_path_planner_html_a5c9fd37284fa20b93f933ce53c932fc6"><div class="ttname"><a href="class_path_planner.html#a5c9fd37284fa20b93f933ce53c932fc6">PathPlanner::get_knots_marker</a></div><div class="ttdeci">visualization_msgs::Marker get_knots_marker()</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00703">PolyTrajGen.cpp:703</a></div></div>
<div class="ttc" id="namespace__setup__util_html_acdce690b925de33d6249bbbfa1109d61"><div class="ttname"><a href="namespace__setup__util.html#acdce690b925de33d6249bbbfa1109d61">_setup_util.e</a></div><div class="ttdeci">e</div><div class="ttdef"><b>Definition:</b> <a href="catkin__generated_2installspace_2__setup__util_8py_source.html#l00261">_setup_util.py:261</a></div></div>
<div class="ttc" id="structtraj__gen_1_1_poly_spline_x_y_z___html_a372cfda50d7d422fa234a2f8ebec21ee"><div class="ttname"><a href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a372cfda50d7d422fa234a2f8ebec21ee">traj_gen::PolySplineXYZ_::is_valid</a></div><div class="ttdeci">_is_valid_type is_valid</div><div class="ttdef"><b>Definition:</b> <a href="_poly_spline_x_y_z_8h_source.html#l00052">PolySplineXYZ.h:52</a></div></div>
<div class="ttc" id="structtraj__gen_1_1_poly_spline_x_y_z___html_a868db24a21948436dd8cbe7c71241bc1"><div class="ttname"><a href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a868db24a21948436dd8cbe7c71241bc1">traj_gen::PolySplineXYZ_::poly_order</a></div><div class="ttdeci">_poly_order_type poly_order</div><div class="ttdef"><b>Definition:</b> <a href="_poly_spline_x_y_z_8h_source.html#l00067">PolySplineXYZ.h:67</a></div></div>
<div class="ttc" id="class_path_planner_html_aa12888c52d7cb2d429c3782060b9b45e"><div class="ttname"><a href="class_path_planner.html#aa12888c52d7cb2d429c3782060b9b45e">PathPlanner::point_eval_spline</a></div><div class="ttdeci">Point point_eval_spline(double t_eval)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00491">PolyTrajGen.cpp:491</a></div></div>
<div class="ttc" id="class_path_planner_html_a1d6a959095513585bfc10468f7b8d180"><div class="ttname"><a href="class_path_planner.html#a1d6a959095513585bfc10468f7b8d180">PathPlanner::get_init_constraint_mat</a></div><div class="ttdeci">Constraint get_init_constraint_mat(double x0, double v0, double a0, TrajGenOpts option)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00676">PolyTrajGen.cpp:676</a></div></div>
<div class="ttc" id="structtraj__gen_1_1_poly_spline_x_y_z___html_a3b5c7122e5190c87c753419d12d526b2"><div class="ttname"><a href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a3b5c7122e5190c87c753419d12d526b2">traj_gen::PolySplineXYZ_::knot_time</a></div><div class="ttdeci">_knot_time_type knot_time</div><div class="ttdef"><b>Definition:</b> <a href="_poly_spline_x_y_z_8h_source.html#l00070">PolySplineXYZ.h:70</a></div></div>
<div class="ttc" id="struct_q_p__form_html_ac8ff933f268484b2f78b77ffc056f11d"><div class="ttname"><a href="struct_q_p__form.html#ac8ff933f268484b2f78b77ffc056f11d">QP_form::H</a></div><div class="ttdeci">MatrixXd H</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00091">PolyTrajGen.h:91</a></div></div>
<div class="ttc" id="_poly_traj_gen_8cpp_html_a17e17da0f42201d595257679c8ae6170"><div class="ttname"><a href="_poly_traj_gen_8cpp.html#a17e17da0f42201d595257679c8ae6170">find_spline_interval</a></div><div class="ttdeci">int find_spline_interval(const vector&lt; double &gt; &amp;ts, double t_eval)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00855">PolyTrajGen.cpp:855</a></div></div>
<div class="ttc" id="structtraj__gen_1_1_poly_spline_x_y_z___html_a1315136ffc90e4164123f07aef230a6e"><div class="ttname"><a href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a1315136ffc90e4164123f07aef230a6e">traj_gen::PolySplineXYZ_::n_seg</a></div><div class="ttdeci">_n_seg_type n_seg</div><div class="ttdef"><b>Definition:</b> <a href="_poly_spline_x_y_z_8h_source.html#l00064">PolySplineXYZ.h:64</a></div></div>
<div class="ttc" id="struct_traj_gen_opts_html_ab4888a8e142d514867bb3dfacb84e3d2"><div class="ttname"><a href="struct_traj_gen_opts.html#ab4888a8e142d514867bb3dfacb84e3d2">TrajGenOpts::verbose</a></div><div class="ttdeci">bool verbose</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00078">PolyTrajGen.h:78</a></div></div>
<div class="ttc" id="struct_traj_gen_opts_html_a7ba33a57327fba375f137f1f0104c1ea"><div class="ttname"><a href="struct_traj_gen_opts.html#a7ba33a57327fba375f137f1f0104c1ea">TrajGenOpts::is_multi_corridor</a></div><div class="ttdeci">bool is_multi_corridor</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00071">PolyTrajGen.h:71</a></div></div>
<div class="ttc" id="structtraj__gen_1_1_poly_spline_x_y_z___html_a149445d98f325cd01096a829e4ac90d8"><div class="ttname"><a href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a149445d98f325cd01096a829e4ac90d8">traj_gen::PolySplineXYZ_::spline_y</a></div><div class="ttdeci">_spline_y_type spline_y</div><div class="ttdef"><b>Definition:</b> <a href="_poly_spline_x_y_z_8h_source.html#l00058">PolySplineXYZ.h:58</a></div></div>
<div class="ttc" id="class_path_planner_html_a35e8872f5ffc5fc3227dc45f9f969a85"><div class="ttname"><a href="class_path_planner.html#a35e8872f5ffc5fc3227dc45f9f969a85">PathPlanner::get_solution</a></div><div class="ttdeci">PolySpline get_solution(VectorXd sol, int poly_order, int n_seg)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00643">PolyTrajGen.cpp:643</a></div></div>
<div class="ttc" id="class_path_planner_html_ad1ad0eb3d19d6f1e9ab921e835361c14"><div class="ttname"><a href="class_path_planner.html#ad1ad0eb3d19d6f1e9ab921e835361c14">PathPlanner::accel_eval_spline</a></div><div class="ttdeci">Twist accel_eval_spline(double t_eval)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00534">PolyTrajGen.cpp:534</a></div></div>
<div class="ttc" id="class_path_planner_html_aff99e738bd54babad38867d7713e5a51"><div class="ttname"><a href="class_path_planner.html#aff99e738bd54babad38867d7713e5a51">PathPlanner::get_continuity_constraint_mat</a></div><div class="ttdeci">Constraint get_continuity_constraint_mat(double dt1, double dt2, TrajGenOpts option)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00727">PolyTrajGen.cpp:727</a></div></div>
<div class="ttc" id="structtraj__gen_1_1_poly_spline___html_a0f2e8bfe261fae6f2b762c093c0d692d"><div class="ttname"><a href="structtraj__gen_1_1_poly_spline__.html#a0f2e8bfe261fae6f2b762c093c0d692d">traj_gen::PolySpline_::poly_coeff</a></div><div class="ttdeci">_poly_coeff_type poly_coeff</div><div class="ttdef"><b>Definition:</b> <a href="_poly_spline_8h_source.html#l00044">PolySpline.h:44</a></div></div>
<div class="ttc" id="struct_q_p__form_html_a949cd1a76929ca7aeb61c8955da4a1a2"><div class="ttname"><a href="struct_q_p__form.html#a949cd1a76929ca7aeb61c8955da4a1a2">QP_form::b</a></div><div class="ttdeci">MatrixXd b</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00093">PolyTrajGen.h:93</a></div></div>
<div class="ttc" id="class_poly_coeff_html"><div class="ttname"><a href="class_poly_coeff.html">PolyCoeff</a></div><div class="ttdef"><b>Definition:</b> <a href="_poly_coeff_8js_source.html#l00017">PolyCoeff.js:17</a></div></div>
<div class="ttc" id="struct_q_p__form_html_a5613317ae662dd137324ee52ca52b1ae"><div class="ttname"><a href="struct_q_p__form.html#a5613317ae662dd137324ee52ca52b1ae">QP_form::Q</a></div><div class="ttdeci">MatrixXd Q</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00090">PolyTrajGen.h:90</a></div></div>
<div class="ttc" id="_poly_traj_gen_8h_html_a37d0493e73d8ef72a2405596ad6cee0c"><div class="ttname"><a href="_poly_traj_gen_8h.html#a37d0493e73d8ef72a2405596ad6cee0c">TimeSeries</a></div><div class="ttdeci">VectorXd TimeSeries</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00030">PolyTrajGen.h:30</a></div></div>
<div class="ttc" id="_poly_traj_gen_8cpp_html_a5344e44ce223789d62117ae55c9a5ac5"><div class="ttname"><a href="_poly_traj_gen_8cpp.html#a5344e44ce223789d62117ae55c9a5ac5">t_vec</a></div><div class="ttdeci">VectorXd t_vec(int poly_order, double t, int n_diff)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00784">PolyTrajGen.cpp:784</a></div></div>
<div class="ttc" id="class_poly_spline_html"><div class="ttname"><a href="class_poly_spline.html">PolySpline</a></div><div class="ttdef"><b>Definition:</b> <a href="_poly_spline_8js_source.html#l00018">PolySpline.js:18</a></div></div>
<div class="ttc" id="class_path_planner_html_ad231acc2bce7d5e143fa49c69515c4a9"><div class="ttname"><a href="class_path_planner.html#ad231acc2bce7d5e143fa49c69515c4a9">PathPlanner::horizon_eval_spline</a></div><div class="ttdeci">void horizon_eval_spline(int N_eval_interval)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00450">PolyTrajGen.cpp:450</a></div></div>
<div class="ttc" id="struct_q_p__form_html_afeb66f78f73f929e99f6f8afcfcc17b1"><div class="ttname"><a href="struct_q_p__form.html#afeb66f78f73f929e99f6f8afcfcc17b1">QP_form::A</a></div><div class="ttdeci">MatrixXd A</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00092">PolyTrajGen.h:92</a></div></div>
<div class="ttc" id="_poly_traj_gen_8h_html"><div class="ttname"><a href="_poly_traj_gen_8h.html">PolyTrajGen.h</a></div></div>
<div class="ttc" id="class_path_planner_html_a6fd581289799d1daac012050fb712a0c"><div class="ttname"><a href="class_path_planner.html#a6fd581289799d1daac012050fb712a0c">PathPlanner::solveqp</a></div><div class="ttdeci">VectorXd solveqp(QP_form qp_prob, bool &amp;is_ok)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00552">PolyTrajGen.cpp:552</a></div></div>
<div class="ttc" id="struct_traj_gen_opts_html_ad4725655d38467206ed000eacfd6c54a"><div class="ttname"><a href="struct_traj_gen_opts.html#ad4725655d38467206ed000eacfd6c54a">TrajGenOpts::is_single_corridor</a></div><div class="ttdeci">bool is_single_corridor</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00070">PolyTrajGen.h:70</a></div></div>
<div class="ttc" id="class_path_planner_html_ae31bac84392ea0fbe59edfc7582215e0"><div class="ttname"><a href="class_path_planner.html#ae31bac84392ea0fbe59edfc7582215e0">PathPlanner::vel_eval_spline</a></div><div class="ttdeci">Twist vel_eval_spline(double t_eval)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00518">PolyTrajGen.cpp:518</a></div></div>
<div class="ttc" id="struct_constraint_html_ab22b0463922cee8c89b0443ed611d072"><div class="ttname"><a href="struct_constraint.html#ab22b0463922cee8c89b0443ed611d072">Constraint::b</a></div><div class="ttdeci">MatrixXd b</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00085">PolyTrajGen.h:85</a></div></div>
<div class="ttc" id="struct_traj_gen_opts_html_a7424679070ea7264f7c8b6df5461bdb9"><div class="ttname"><a href="struct_traj_gen_opts.html#a7424679070ea7264f7c8b6df5461bdb9">TrajGenOpts::w_d</a></div><div class="ttdeci">double w_d</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00068">PolyTrajGen.h:68</a></div></div>
<div class="ttc" id="struct_q_p__form__xyz_html_a3a6e7a104ef31fda66007013ba3741f8"><div class="ttname"><a href="struct_q_p__form__xyz.html#a3a6e7a104ef31fda66007013ba3741f8">QP_form_xyz::x</a></div><div class="ttdeci">QP_form x</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00100">PolyTrajGen.h:100</a></div></div>
<div class="ttc" id="struct_traj_gen_opts_html_ad8937f645e1154dc52978bd6592ce836"><div class="ttname"><a href="struct_traj_gen_opts.html#ad8937f645e1154dc52978bd6592ce836">TrajGenOpts::objective_derivative</a></div><div class="ttdeci">int objective_derivative</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00065">PolyTrajGen.h:65</a></div></div>
<div class="ttc" id="struct_traj_gen_opts_html_a216fe9912362cc5e876f945112811c35"><div class="ttname"><a href="struct_traj_gen_opts.html#a216fe9912362cc5e876f945112811c35">TrajGenOpts::N_safe_pnts</a></div><div class="ttdeci">int N_safe_pnts</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00076">PolyTrajGen.h:76</a></div></div>
<div class="ttc" id="struct_traj_gen_opts_html_a5f27ab3f06720accfeaf88d44da2c5d6"><div class="ttname"><a href="struct_traj_gen_opts.html#a5f27ab3f06720accfeaf88d44da2c5d6">TrajGenOpts::is_waypoint_soft</a></div><div class="ttdeci">bool is_waypoint_soft</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00067">PolyTrajGen.h:67</a></div></div>
<div class="ttc" id="structtraj__gen_1_1_poly_spline_x_y_z___html_a60e87b9c4f18726568ef29bb43fba580"><div class="ttname"><a href="structtraj__gen_1_1_poly_spline_x_y_z__.html#a60e87b9c4f18726568ef29bb43fba580">traj_gen::PolySplineXYZ_::spline_z</a></div><div class="ttdeci">_spline_z_type spline_z</div><div class="ttdef"><b>Definition:</b> <a href="_poly_spline_x_y_z_8h_source.html#l00061">PolySplineXYZ.h:61</a></div></div>
<div class="ttc" id="class_path_planner_html_a4b5f5beea2c380ae4340f65dd1d5bd10"><div class="ttname"><a href="class_path_planner.html#a4b5f5beea2c380ae4340f65dd1d5bd10">PathPlanner::qp_gen</a></div><div class="ttdeci">QP_form_xyz qp_gen(const TimeSeries &amp;knots, const nav_msgs::Path &amp;waypoints, const geometry_msgs::Twist &amp;v0, const geometry_msgs::Twist &amp;a0, TrajGenOpts opt)</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8cpp_source.html#l00107">PolyTrajGen.cpp:107</a></div></div>
<div class="ttc" id="struct_traj_gen_opts_html_a538985230ef4a8cd7763c91d11bebbaf"><div class="ttname"><a href="struct_traj_gen_opts.html#a538985230ef4a8cd7763c91d11bebbaf">TrajGenOpts::poly_order</a></div><div class="ttdeci">int poly_order</div><div class="ttdef"><b>Definition:</b> <a href="_poly_traj_gen_8h_source.html#l00073">PolyTrajGen.h:73</a></div></div>
</div><!-- fragment --></div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.11
</small></address>
</body>
</html>
